diff --git a/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
index da9044272ea330e61d090e0410d1ab4f7ca4b64d..67647069d1ddbd9d7e8499ee3acdef9ce9da95b7 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
@@ -168,10 +168,6 @@ float yaml_cf_mass_in_grams = 25.0;
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81 / 1000.0;
-// > the coefficients of the 16-bit command to thrust conversion
-//std::vector<float> yaml_motorPoly(3);
-std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
-
// > the frequency at which the controller is running
float yaml_control_frequency = 200.0;
@@ -237,9 +233,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// INTO AN (x,y) BODY FRAME ERROR
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
-// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
-float computeMotorPolyBackward(float thrust);
-
// REQUEST SETPOINT CHANGE CALLBACK
void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index cccd6020e3fc39aedd190ba90a19c3bdce133683..fe0977ca4097fc1e664f0fa158e10418db05fe82 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -159,28 +159,13 @@
/>
<rosparam
command = "load"
- file = "$(find dfall_pkg)/param/SafeController.yaml"
- ns = "SafeController"
+ file = "$(find dfall_pkg)/param/DefaultController.yaml"
+ ns = "DefaultController"
/>
<rosparam
command = "load"
- file = "$(find dfall_pkg)/param/RemoteController.yaml"
- ns = "RemoteController"
- />
- <rosparam
- command = "load"
- file = "$(find dfall_pkg)/param/TuningController.yaml"
- ns = "TuningController"
- />
- <rosparam
- command = "load"
- file = "$(find dfall_pkg)/param/PickerController.yaml"
- ns = "PickerController"
- />
- <rosparam
- command = "load"
- file = "$(find dfall_pkg)/param/TemplateController.yaml"
- ns = "TemplateController"
+ file = "$(find dfall_pkg)/param/StudentController.yaml"
+ ns = "StudentController"
/>
</node>
diff --git a/dfall_ws/src/dfall_pkg/param/StudentController.yaml b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
index 9328045bc657c15ec4e183e46a5d00398da7c507..8a867e07c5b6a454d926ffb792ae4692a22b19e7 100644
--- a/dfall_ws/src/dfall_pkg/param/StudentController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
@@ -4,9 +4,6 @@ mass : 28
# Frequency of the controller, in hertz
control_frequency : 200
-# Quadratic motor regression equation (a0, a1, a2)
-motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
-
# The default setpoint, the ordering is (x,y,z,yaw),
# with unit [meters,meters,meters,radians]
default_setpoint : [0.0, 0.0, 0.4, 0.0]
diff --git a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
index b61a3e84ba4f77f9e3e914e2d3741baeb4212d21..6e9de18ca30ef0ded472a05f1915839aaef1ea73 100644
--- a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
+++ b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
@@ -1,4 +1,11 @@
dictionary : {
- 'FlyingAgentClientConfig' : 'FlyingAgentClientConfig' ,
- 'SafeController' : 'SafeController'
+ 'BatteryMonitor' : 'BatteryMonitor',
+ 'FlyingAgentClientConfig' : 'FlyingAgentClientConfig',
+ 'CsoneControllerService' : 'CsoneControllerService',
+ 'DefaultControllerService' : 'DefaultControllerService',
+ 'PickerControllerService' : 'PickerControllerService',
+ 'RemoteControllerService' : 'RemoteControllerService',
+ 'StudentControllerService' : 'StudentControllerService',
+ 'TemplateControllerService' : 'TemplateControllerService',
+ 'TuningControllerService' : 'TuningControllerService'
}
diff --git a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
index ff6f9bc80627722a900770188e4a774e5141bf62..c299932b98a82a2610be415b7c806847f9ab2091 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
@@ -272,9 +272,16 @@ float newtons2cmd_for_crazyflie( float thrust)
// Clip the cmd_16bit to avoid wrapping
if (cmd_16bit > 60000)
+ {
cmd_16bit = 60000;
- else if (cmd_16bit < 2000)
- cmd_16bit = 0;
+ }
+ else
+ {
+ if (cmd_16bit < 2000)
+ {
+ cmd_16bit = 0;
+ }
+ }
// Return the result
return cmd_16bit;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index 1b64bf0e5ecb77820c7545ff00747eab8637f242..c4619227eb64d57c2e6940996e7bd689490ebafa 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -395,8 +395,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Put the computed roll rate into the "response" variable
// > The "controller mode" specifies that it is an
// angular rate setpoint
- response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
- response.controlOutput.xControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
@@ -848,10 +848,6 @@ void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle)
// by the frequency at which the Vicon system provides position and attitude data
yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
- // > The co-efficients of the quadratic conversation from 16-bit motor command to
- // thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
-
// The default setpoint, the ordering is (x,y,z,yaw),
// with unit [meters,meters,meters,radians]
getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
@@ -981,10 +977,6 @@ int main(int argc, char* argv[]) {
// > the mass of the crazyflie, in [grams]
yaml_cf_mass_in_grams = 25.0;
- // > the coefficients of the 16-bit command to thrust conversion
- yaml_motorPoly[0] = 5.484560e-4;
- yaml_motorPoly[1] = 1.032633e-6;
- yaml_motorPoly[2] = 2.130295e-11;
// > the frequency at which the controller is running
yaml_control_frequency = 200.0;
// > the default setpoint, the ordering is (x,y,z,yaw),