From 36c16643229bfd743226018e91b8ea61d193c923 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Thu, 28 Jun 2018 18:41:38 +0200
Subject: [PATCH] Finished implmementing and testing the Tuning contorller
---
.../GUI_Qt/studentGUI/src/MainWindow.ui | 6 +-
.../include/nodes/TuningControllerService.h | 37 ++++++++
pps_ws/src/d_fall_pps/launch/Agent.launch | 14 +++
pps_ws/src/d_fall_pps/param/ClientConfig.yaml | 4 +-
.../d_fall_pps/param/TuningController.yaml | 27 ++++--
.../src/nodes/TuningControllerService.cpp | 89 +++++++++++++------
6 files changed, 135 insertions(+), 42 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 58644b6d..5e7ed897 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -2833,7 +2833,7 @@
<item row="4" column="0">
<widget class="QLabel" name="label_45">
<property name="text">
- <string>Vertical Controller Gain</string>
+ <string>Vertical Controller Gain (Vertikal Regler Verstaekung)</string>
</property>
</widget>
</item>
@@ -2905,7 +2905,7 @@
<item row="8" column="0">
<widget class="QLabel" name="label_46">
<property name="text">
- <string>Heading Controller Gain</string>
+ <string>Heading Controller Gain (Orientierung Regler Verstaekung)</string>
</property>
</widget>
</item>
@@ -2998,7 +2998,7 @@
<item row="0" column="0">
<widget class="QLabel" name="label_37">
<property name="text">
- <string>Horizontal Controller Gain</string>
+ <string>Horizontal Controller Gain (Horizontal Regler Verstaekung)</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
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 71068b10..b89c0661 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -202,6 +202,19 @@ int test_all_currentpoint = 1;
std::vector<float> test_all_setpoint1 (4,0.0);
std::vector<float> test_all_setpoint2 (4,0.0);
+// Multipliers for the HORIZONTAL contorller
+float multiplier_horizontal = 1.0;
+float multiplier_horizontal_min = 0.9;
+float multiplier_horizontal_max = 1.1;
+// Multipliers for the VERTICAL contorller
+float multiplier_vertical = 1.0;
+float multiplier_vertical_min = 0.9;
+float multiplier_vertical_max = 1.1;
+// Multipliers for the HEADING contorller
+float multiplier_heading = 1.0;
+float multiplier_heading_min = 0.9;
+float multiplier_heading_max = 1.1;
+
// LQR Gain matrix for tracking the angle commands from the Crazyflie
std::vector<float> gainMatrixRollRate_forRemoteControl (3,0.0);
@@ -270,6 +283,30 @@ float lqr_angleRateNested_prev_pitchAngle = 0.0;
float lqr_angleRateNested_prev_yawAngle = 0.0;
+// // SAME GAINS AGAIN BUT INSTEAD AS DEFAULTS
+
+
+// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+// std::vector<float> gainMatrixThrust_NineStateVector_default (9,0.0);
+// std::vector<float> gainMatrixRollRate_default (9,0.0);
+// std::vector<float> gainMatrixPitchRate_default (9,0.0);
+// std::vector<float> gainMatrixYawRate_default (9,0.0);
+
+// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
+// std::vector<float> gainMatrixThrust_SixStateVector_default (6,0.0);
+// std::vector<float> gainMatrixRollAngle_default (6,0.0);
+// std::vector<float> gainMatrixPitchAngle_default (6,0.0);
+
+// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
+// std::vector<float> gainMatrixThrust_SixStateVector_50Hz_default (6,0.0);
+// std::vector<float> gainMatrixRollAngle_50Hz_default (6,0.0);
+// std::vector<float> gainMatrixPitchAngle_50Hz_default (6,0.0);
+
+// std::vector<float> gainMatrixRollRate_Nested_default (3,0.0);
+// std::vector<float> gainMatrixPitchRate_Nested_default (3,0.0);
+// std::vector<float> gainMatrixYawRate_Nested_default (3,0.0);
+
+
// The 16-bit command limits
float cmd_sixteenbit_min;
float cmd_sixteenbit_max;
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index ecc4b1be..a42e7b46 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -74,6 +74,15 @@
>
</node>
+ <!-- TUNING CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "TuningControllerService"
+ output = "screen"
+ type = "TuningControllerService"
+ >
+ </node>
+
<!-- PARAMETER SERVICE -->
<node
pkg = "d_fall_pps"
@@ -113,6 +122,11 @@
file = "$(find d_fall_pps)/param/RemoteController.yaml"
ns = "RemoteController"
/>
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/TuningController.yaml"
+ ns = "TuningController"
+ />
</node>
<!-- AGENT GUI (aka. the "student GUI") -->
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index c525b101..86e9ec7c 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -5,8 +5,8 @@ mpcController: "MpcControllerService/MpcController"
remoteController: "RemoteControllerService/RemoteController"
tuningController: "TuningControllerService/TuningController"
-strictSafety: false
-angleMargin: 0.6
+strictSafety: true
+angleMargin: 0.8
battery_threshold_while_flying: 2.8 # in V
battery_threshold_while_motors_off: 3.30 # in V
diff --git a/pps_ws/src/d_fall_pps/param/TuningController.yaml b/pps_ws/src/d_fall_pps/param/TuningController.yaml
index c03384f7..487afa22 100644
--- a/pps_ws/src/d_fall_pps/param/TuningController.yaml
+++ b/pps_ws/src/d_fall_pps/param/TuningController.yaml
@@ -2,20 +2,31 @@
# PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
# Setpoint for the HORIZONTAL test
-test_horizontal_setpoint1 : [0.3, 0.0, 0.4, 0.0];
-test_horizontal_setpoint2 : [-0.3, 0.0, 0.4, 0.0];
+test_horizontal_setpoint1 : [0.3, 0.0, 0.4, 0.0]
+test_horizontal_setpoint2 : [-0.3, 0.0, 0.4, 0.0]
# Setpoint for the VERTICAL test
-test_vertical_setpoint1 : [0.0, 0.0, 0.4, 0.0];
-test_vertical_setpoint2 : [0.0, 0.0, 0.8, 0.0];
+test_vertical_setpoint1 : [0.0, 0.0, 0.4, 0.0]
+test_vertical_setpoint2 : [0.0, 0.0, 0.8, 0.0]
# Setpoint for the HEADING test
-test_heading_setpoint1 : [0.0, 0.0, 0.4, 0.0];
-test_heading_setpoint2 : [0.0, 0.0, 0.4, 1.5];
+test_heading_setpoint1 : [0.0, 0.0, 0.4, 0.0]
+test_heading_setpoint2 : [0.0, 0.0, 0.4, 3.14]
# Setpoint for the ALL test
-test_all_setpoint1 : [0.3, 0.0, 0.4, 0.0];
-test_all_setpoint2 : [-0.3, 0.0, 0.8, 1.5];
+test_all_setpoint1 : [0.3, 0.0, 0.4, 0.0]
+test_all_setpoint2 : [-0.3, 0.0, 0.8, 3.14]
+
+# Multipliers for the HORIZONTAL contorller
+multiplier_horizontal_min : 0.1
+multiplier_horizontal_max : 2.2
+# Multipliers for the VERTICAL contorller
+multiplier_vertical_min : 0.1
+multiplier_vertical_max : 1.8
+# Multipliers for the HEADING contorller
+multiplier_heading_min : 0.05
+multiplier_heading_max : 3.0
+
# ***************************************************************************** #
diff --git a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
index 815e664b..4a0648e7 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
@@ -232,6 +232,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test the HORIZONTAL controller
case 1:
{
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform HORIZONTAL test");
+
switch (test_horizontal_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -269,6 +272,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test the VERTICAL controller
case 2:
{
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform VERTICAL test");
+
switch (test_vertical_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -306,6 +312,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test the HEADING controller
case 3:
{
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform HEADING test");
+
switch (test_heading_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -343,6 +352,9 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Test ALL the controllers
case 4:
{
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform ALL test");
+
switch (test_all_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -390,19 +402,40 @@ void activateTestCallback(const std_msgs::Int32& msg)
// CHANGE THE GAIN FOR THE HORIZONTAL CONTROLLER
void horizontalGainCallback(const std_msgs::Int32& msg)
{
-
+ // Get the value from the message
+ float value = float(msg.data) / 1000.0;
+
+ // Compute the new multiplier value
+ multiplier_horizontal = multiplier_horizontal_min + value * (multiplier_horizontal_max-multiplier_horizontal_min);
+
+ // Display the value received
+ ROS_INFO_STREAM("[TUNING CONTROLLER] Received new HORIZONTAL gain with value " << value << ", the multiplier now = " << multiplier_horizontal );
}
// CHANGE THE GAIN FOR THE VERTICAL CONTROLLER
void verticalGainCallback(const std_msgs::Int32& msg)
{
-
+ // Get the value from the message
+ float value = float(msg.data) / 1000.0;
+
+ // Compute the new multiplier value
+ multiplier_vertical = multiplier_vertical_min + value * (multiplier_vertical_max-multiplier_vertical_min);
+
+ // Display the value received
+ ROS_INFO_STREAM("[TUNING CONTROLLER] Received new VERTICAL gain with value " << value << ", the multiplier now = " << multiplier_vertical );
}
// CHANGE THE GAIN FOR THE HEADING CONTROLLER
void headingGainCallback(const std_msgs::Int32& msg)
{
-
+ // Get the value from the message
+ float value = float(msg.data) / 1000.0;
+
+ // Compute the new multiplier value
+ multiplier_heading = multiplier_heading_min + value * (multiplier_heading_max-multiplier_heading_min);
+
+ // Display the value received
+ ROS_INFO_STREAM("[TUNING CONTROLLER] Received new HEADING gain with value " << value << ", the multiplier now = " << multiplier_heading );
}
@@ -633,13 +666,13 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
for(int i = 0; i < 9; ++i)
{
// BODY FRAME Y CONTROLLER
- rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
- pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME YAW CONTROLLER
- yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i] * multiplier_heading;
// > ALITUDE CONTROLLER (i.e., z-controller):
- thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i] * multiplier_vertical;
}
@@ -722,11 +755,11 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
- rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
+ rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
- pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
+ pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i] * multiplier_horizontal;
// > ALITUDE CONTROLLER (i.e., z-controller):
- thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
+ thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i] * multiplier_vertical;
}
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
@@ -789,11 +822,11 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
- lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
+ lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
- lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
+ lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// > ALITUDE CONTROLLER (i.e., z-controller):
- lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
+ lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i] * multiplier_vertical;
}
// BODY FRAME Z CONTROLLER
@@ -830,7 +863,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// BODY FRAME X CONTROLLER
pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
// BODY FRAME Z CONTROLLER
- yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
+ yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i] * multiplier_heading;
}
@@ -1252,8 +1285,19 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint1", test_all_setpoint1, 4);
getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint2", test_all_setpoint2, 4);
+
+ // Multipliers for the HORIZONTAL contorller
+ multiplier_horizontal_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_horizontal_min");
+ multiplier_horizontal_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_horizontal_max");
+ // Multipliers for the VERTICAL contorller
+ multiplier_vertical_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_vertical_min");
+ multiplier_vertical_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_vertical_max");
+ // Multipliers for the HEADING contorller
+ multiplier_heading_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_heading_min");
+ multiplier_heading_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_heading_max");
+
// // DEBUGGING: Print out one of the parameters that was loaded
- // ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/default_viconObjectName_forRemote = " << default_viconObjectName_forRemote);
+ ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: the fetched TuningController/multiplier_horizontal_min = " << multiplier_horizontal_min);
// ******************************************************************************* //
@@ -1355,21 +1399,8 @@ void processFetchedParameters()
// Set that the estimator frequency is the same as the control frequency
estimator_frequency = vicon_frequency;
-
- // // Roll and pitch limit (in degrees for angles)
- // remoteControlLimit_roll = remoteControlLimit_roll_degrees * PI / 180.0;
- // remoteControlLimit_pitch = remoteControlLimit_pitch_degrees * PI / 180.0;
-
-
- // // Use the Remote name if the variable is currently empty
- // if (viconObjectName_forRemote == "empty")
- // {
- // viconObjectName_forRemote = default_viconObjectName_forRemote;
- // }
-
-
// DEBUGGING: Print out one of the computed quantities
- //ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: after processing the viconObjectName_forRemote = " << viconObjectName_forRemote);
+ ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: after processing the gravity_force_quarter = " << gravity_force_quarter);
}
--
GitLab