diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
index 030011304d4c00366c8bdac3ee9c44a37acf1106..96a9f12bc5654d80e9f2cfa115303467457f675e 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
@@ -48,7 +48,6 @@
#include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h"
-#include "d_fall_pps/CustomControllerYAML.h"
#include <std_msgs/Int32.h>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
index 21eaae66e4b8d490e04203f0352cf53461ca6792..a8adb30557c94b51091ee23ece23a8bfba345ead 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
@@ -63,7 +63,6 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/ViconSubscribeObjectName.h"
-#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
diff --git a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
index 9c15a5341b35fe8dba9d6e9550d78d8f8b9b12e1..818993f0758f58f4a980ab1352944764831bf6a0 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -56,7 +56,6 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
-#include "d_fall_pps/CustomControllerYAML.h"
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
@@ -127,12 +126,12 @@ using namespace d_fall_pps;
// ----------------------------------------------------------------------------------
// Variables for controller
-float cf_mass; // Crazyflie mass in grams
-std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
-float control_frequency = 200.0; // Frequency at which the controller is running
-float gravity_force = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
+float cf_mass_in_grams = 25.0; // Crazyflie mass in grams
+std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
+float control_frequency = 200.0; // Frequency at which the controller is running
+float cf_weight_in_newtons = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
-float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
+float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
diff --git a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
index 1bc722d03614311c8d55921492096e5750485cd3..9866bdbe93c3a79cbc59c8470aaf75ea08b8cd8b 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -63,7 +63,6 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/ViconSubscribeObjectName.h"
-#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index c65ad4e7b1648ed8258bca92bbb537e574e9ceb3..a9616e9d9abc2f565f79103502a8f815dcae32b3 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1 +1,8 @@
-4,PPS_CF03,0/16/2M/E7E7E7E703,0,0.345924,-1.31629,-0.2,4.30616,1.15886,1.2
+1,CF01,0/0/2M/E7E7E7E701,0,-1.95374,-0.0143279,-0.2,-1.01034,0.774692,1.2
+2,CF02,0/8/2M/E7E7E7E702,1,-0.844536,-0.025763,-0.2,0.0759869,0.751821,1.2
+3,CF03,0/16/2M/E7E7E7E703,2,0.213208,0.00282468,-0.2,1.18519,0.757539,1.2
+4,CF04,0/24/2M/E7E7E7E704,3,1.34528,-0.0143279,-0.2,2.31154,0.723234,1.2
+5,CF05,0/32/2M/E7E7E7E705,4,1.351,0.791844,-0.2,2.31154,1.54084,1.2
+6,CF06,0/40/2M/E7E7E7E706,5,0.20749,0.843302,-0.2,1.18519,1.55799,1.2
+7,CF07,0/48/2M/E7E7E7E707,6,-0.867406,0.843302,-0.2,0.0702693,1.54656,1.2
+8,CF08,0/56/2M/E7E7E7E708,7,-1.95374,0.854737,-0.2,-0.987474,1.54656,1.2
diff --git a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
index 4a4978ccd7677868e38dde86cca1bab2a0704c88..9195d629a3266051b94d64da16535a1f4ae22faf 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -56,7 +56,6 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
-#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
diff --git a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index a9eec98a20b1c8f8d5910b0e77956b10cbbc2d1e..8ba28bbf9522bd642d710a54777f1641ecd84a7a 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -268,13 +268,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
- // > NOTE: the "gravity_force" value was already divided by 4 when is was
- // loaded from the .yaml file via the "fetchYamlParameters"
- // class function in this file.
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+ // > NOTE: the "cf_weight_in_newtons" value is the total thrust required
+ // as feed-forward. Assuming the the Crazyflie is symmetric, this
+ // value is divided by four.
+ float feed_forward_thrust_per_motor = cf_weight_in_newtons / 4.0;
+ // > NOTE: the function "computeMotorPolyBackward" converts the input argument
+ // from Newtons to the 16-bit command expected by the Crazyflie.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
@@ -665,10 +668,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
ros::NodeHandle nodeHandle_for_studentController(nodeHandle, "StudentController");
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_studentController , "mass");
+ cf_mass_in_grams = getParameterFloat(nodeHandle_for_studentController , "mass");
// Display one of the YAML parameters to debug if it is working correctly
- //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass_in_grams );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
@@ -680,7 +683,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass_in_grams);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -696,10 +699,10 @@ void processFetchedParameters()
{
// Compute the feed-forward force that we need to counteract gravity (i.e., mg)
// > in units of [Newtons]
- gravity_force = cf_mass * 9.81/(1000*4);
+ cf_weight_in_newtons = cf_mass_in_grams * 9.81/(1000*4);
// DEBUGGING: Print out one of the computed quantities
- ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the graity force = " << gravity_force);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << cf_weight_in_newtons);
}