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 a95e30748ff112c301378224a6ccb8837b89ddba..234d8ec4473c10224f0308a70737385dae40a4c5 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -128,8 +128,8 @@ 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; // Frequency at which the controller is running
-float gravity_force; // The weight of the Crazyflie in Newtons, i.e., mg
+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 previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
@@ -137,10 +137,10 @@ std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z)
// The LQR Controller parameters for "LQR_RATE_MODE"
-const float gainMatrixRollRate[9] = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
-const float gainMatrixPitchRate[9] = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
-const float gainMatrixYawRate[9] = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
-const float gainMatrixThrust[9] = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
+std::vector<float> gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
+std::vector<float> gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
+std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
+std::vector<float> gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
// ROS Publisher for debugging variables
diff --git a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
index 0cdc62f005976b6ba557a95aeef0fcf5ea367d0a..824d6c99e5a99fd14609bbc6254d08ac7285f4c7 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
@@ -960,17 +960,17 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Cont
// > body frame yaw angle,
// > total thrust adjustment.
// These will be requested from the "inner-loop" LQR controller below
- angleResponseTest_prev_rollAngle = 0;
- //angleResponseTest_prev_pitchAngle = 0;
+ //angleResponseTest_prev_rollAngle = 0;
+ angleResponseTest_prev_pitchAngle = 0;
angleResponseTest_prev_thrustAdjustment = 0;
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
- angleResponseTest_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
+ //angleResponseTest_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
- //angleResponseTest_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
+ angleResponseTest_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
// > ALITUDE CONTROLLER (i.e., z-controller):
angleResponseTest_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
}
@@ -980,13 +980,23 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Cont
angleResponseTest_prev_yawAngle = stateErrorBody[8];
// COMPUTE THE DISTANCE FROM THE ORIGIN
- if (stateErrorBody[0] > angleResponseTest_distance_meters)
+ // > for pitch response testing
+ // if (stateErrorBody[0] > angleResponseTest_distance_meters)
+ // {
+ // angleResponseTest_prev_pitchAngle = -angleResponseTest_pitchAngle_radians;
+ // }
+ // else if (stateErrorBody[0] < (-angleResponseTest_distance_meters) )
+ // {
+ // angleResponseTest_prev_pitchAngle = angleResponseTest_pitchAngle_radians;
+ // }
+ // > for roll response testing
+ if (stateErrorBody[1] > angleResponseTest_distance_meters)
{
- angleResponseTest_prev_pitchAngle = -angleResponseTest_pitchAngle_radians;
+ angleResponseTest_prev_rollAngle = angleResponseTest_pitchAngle_radians;
}
- else if (stateErrorBody[0] < (-angleResponseTest_distance_meters) )
+ else if (stateErrorBody[1] < (-angleResponseTest_distance_meters) )
{
- angleResponseTest_prev_pitchAngle = angleResponseTest_pitchAngle_radians;
+ angleResponseTest_prev_rollAngle = -angleResponseTest_pitchAngle_radians;
}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
index c361f4cdcb6daa64158b3a0cc1758bbe0d8b6e3e..6607ee9a43ae794b655342bea64395021b1df61d 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
@@ -121,6 +121,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//INFORMATION: this ugly fix was needed for the older firmware
//outYaw *= 0.5;
+ //ROS_INFO_STREAM("y-error = " << state[1]);
+ //ROS_INFO_STREAM("y-velocity = " << state[4]);
+ //ROS_INFO_STREAM("roll = " << state[6]);
+ //ROS_INFO_STREAM("rollRate = " << outRoll );
+
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
@@ -492,22 +497,24 @@ int main(int argc, char* argv[]) {
// Call the class function that loads the parameters for this class.
// Sleep for some time (in seconds)
- ros::Duration(1.0).sleep();
+ // ros::Duration(1.0).sleep();
+
+ // // Ask the Paramter Service to load the respective YAML file
+ // std::string namespace_to_own_agent_loadYamlFiles_service = namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
+ // loadYamlFilesService_own_agent = ros::service::createClient<LoadYamlFiles>(namespace_to_own_agent_loadYamlFiles_service, true);
+ // ROS_INFO_STREAM("[SAFE CONTROLLER] Loaded service: " << loadYamlFilesService_own_agent.getService());
- // Ask the Paramter Service to load the respective YAML file
- std::string namespace_to_own_agent_loadYamlFiles_service = namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
- loadYamlFilesService_own_agent = ros::service::createClient<LoadYamlFiles>(namespace_to_own_agent_loadYamlFiles_service, true);
- ROS_INFO_STREAM("[SAFE CONTROLLER] Loaded service: " << loadYamlFilesService_own_agent.getService());
+ // LoadYamlFiles loadYamlFilesService;
+ // std::vector<std::string> yamlFileNames_to_load = {"SafeController"};
+ // loadYamlFilesService.request.yamlFileNames = yamlFileNames_to_load;
+ // bool success = loadYamlFilesService_own_agent.call(loadYamlFilesService);
- LoadYamlFiles loadYamlFilesService;
- std::vector<std::string> yamlFileNames_to_load = {"SafeController"};
- loadYamlFilesService.request.yamlFileNames = yamlFileNames_to_load;
- bool success = loadYamlFilesService_own_agent.call(loadYamlFilesService);
+ // ROS_INFO_STREAM("[SAFE CONTROLLER] called Laod Yaml File service with success = " << success);
- ROS_INFO_STREAM("[SAFE CONTROLLER] called Laod Yaml File service with success = " << success);
+ // ros::Duration(2.0).sleep();
- ros::Duration(2.0).sleep();
+ // Call the class function that loads the parameters for this class.
fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
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 24623df5a48712ad4de438c997f4e7fbcf8bdc6c..fe53de2eb2372965342059146fa26e4c3e3cdc99 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -73,7 +73,7 @@
// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
// ----------------------------------------------------------------------------------
-// This function is the callback that is linked to the "CustomController" service that
+// This function is the callback that is linked to the "StudentController" service that
// is advertised in the main function. This must have arguments that match the
// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
// folder)
@@ -276,8 +276,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
-
-
+
// **************************************
// BBBB OOO DDDD Y Y X X
@@ -326,9 +325,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Put the computed roll rate into the "response" variable
response.controlOutput.roll = rollRate_forResponse;
-
-
-
+
+
// PREPARE AND RETURN THE VARIABLE "response"
/*choosing the Crazyflie onBoard controller type.
@@ -579,7 +577,7 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
default:
{
// Let the user know that the command was not relevant
- //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("The StudentControllerService received the message that YAML parameters were (re-)loaded");
//ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
break;
}
@@ -592,28 +590,28 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
// your controller easier and quicker.
void fetchYamlParameters(ros::NodeHandle& nodeHandle)
{
- // Here we load the parameters that are specified in the CustomController.yaml file
+ // Here we load the parameters that are specified in the StudentController.yaml file
- // Add the "CustomController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_customController(nodeHandle, "StudentController");
+ // Add the "StudentController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_studentController(nodeHandle, "StudentController");
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
+ cf_mass = 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 );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- control_frequency = getParameterFloat(nodeHandle_for_customController, "control_frequency");
+ control_frequency = getParameterFloat(nodeHandle_for_studentController, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_customController, "motorPoly", motorPoly, 3);
+ getParameterFloatVector(nodeHandle_for_studentController, "motorPoly", motorPoly, 3);
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -805,7 +803,7 @@ int main(int argc, char* argv[]) {
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
// Call the class function that loads the parameters for this class.
- //fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
// *********************************************************************************
@@ -823,7 +821,7 @@ int main(int argc, char* argv[]) {
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
// Instantiate the local variable "service" to be a "ros::ServiceServer" type
- // variable that advertises the service called "CustomController". This service has
+ // variable that advertises the service called "StudentController". This service has
// the input-output behaviour defined in the "Controller.srv" file (located in the
// "srv" folder). This service, when called, is provided with the most recent
// measurement of the Crazyflie and is expected to respond with the control action