Corrected small error in the Student controller for confirming with the new...

Corrected small error in the Student controller for confirming with the new Control Command message definition. Cleaned up a few small things at the same time.

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);
 

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>
 

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]

dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml

 dictionary : {
-  'FlyingAgentClientConfig'   : 'FlyingAgentClientConfig' ,
-  'SafeController' : 'SafeController'
+  'BatteryMonitor'             : 'BatteryMonitor',
+  'FlyingAgentClientConfig'    : 'FlyingAgentClientConfig',
+  'CsoneControllerService'     : 'CsoneControllerService',
+  'DefaultControllerService'   : 'DefaultControllerService',
+  'PickerControllerService'    : 'PickerControllerService',
+  'RemoteControllerService'    : 'RemoteControllerService',
+  'StudentControllerService'   : 'StudentControllerService',
+  'TemplateControllerService'  : 'TemplateControllerService',
+  'TuningControllerService'    : 'TuningControllerService'
 }

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;	

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),