diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index cea18ddcd5dc1c8098df34e4ed5c4211beff7000..bdf36d3557a140bb9c369a58c83f851c93abf05a 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -173,6 +173,7 @@ private slots:
void on_tuning_test_vertical_button_clicked();
void on_tuning_test_heading_button_clicked();
void on_tuning_test_all_button_clicked();
+ void on_tuning_test_circle_button_clicked();
void on_tuning_slider_horizontal_valueChanged(int value);
void on_tuning_slider_vertical_valueChanged(int value);
void on_tuning_slider_heading_valueChanged(int value);
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index afedd1f4546fb4f3e0e8bacd7ba91f3c068e7261..3a3d12664e4571d3c62d7039f323dda6388e57c8 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -1299,6 +1299,16 @@ void MainWindow::on_tuning_test_all_button_clicked()
this->tuningActivateTestPublisher.publish(msg);
}
+void MainWindow::on_tuning_test_circle_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 5;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
+
void MainWindow::on_tuning_slider_horizontal_valueChanged(int value)
{
// Initialise the message
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 5e7ed897646ffb603bd8f92fd9fad8296af4218b..6181841c71a0a022295a027cb2250b32276585a6 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
@@ -2939,7 +2939,7 @@
</property>
<property name="maximumSize">
<size>
- <width>400</width>
+ <width>340</width>
<height>50</height>
</size>
</property>
@@ -2948,6 +2948,25 @@
</property>
</widget>
</item>
+ <item>
+ <widget class="QPushButton" name="tuning_test_circle_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>340</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Fly a circle</string>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
<item row="6" column="0">
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 b89c06618751df4346473349e662a25df36d3c50..1bc722d03614311c8d55921492096e5750485cd3 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -202,16 +202,25 @@ int test_all_currentpoint = 1;
std::vector<float> test_all_setpoint1 (4,0.0);
std::vector<float> test_all_setpoint2 (4,0.0);
+// Parameters for flying in a circle
+float test_circle_radius = 0.2;
+float test_circle_seconds_per_rev = 5.0;
+float test_circle_height = 0.4;
+float test_circle_pirouette_per_rev = 0.0;
+float test_circle_time_to_reach_start = 0.7;
+bool isActive_fly_test_circle = false;
+int test_circle_ticks_since_start = 0;
+
// Multipliers for the HORIZONTAL contorller
-float multiplier_horizontal = 1.0;
+float multiplier_horizontal = 0.1;
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 = 0.1;
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 = 0.1;
float multiplier_heading_min = 0.9;
float multiplier_heading_max = 1.1;
@@ -481,6 +490,8 @@ void processFetchedParameters();
// ACTIVATE THE TESTS
void activateTestCallback(const std_msgs::Int32& msg);
+// UPDATE SETPOINT FOR CIRCLE TEST
+void update_setpoint_for_test_circle();
// CHANGE THE GAINS
void horizontalGainCallback(const std_msgs::Int32& msg);
void verticalGainCallback(const std_msgs::Int32& msg);
diff --git a/pps_ws/src/d_fall_pps/param/TuningController.yaml b/pps_ws/src/d_fall_pps/param/TuningController.yaml
index 487afa2226a1a522ed4c5734f1a0f614d16742cf..0bf1cb89869c7d2462bfd4db9f5d5162f1905a2d 100644
--- a/pps_ws/src/d_fall_pps/param/TuningController.yaml
+++ b/pps_ws/src/d_fall_pps/param/TuningController.yaml
@@ -7,7 +7,7 @@ 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_setpoint2 : [0.0, 0.0, 0.7, 0.0]
# Setpoint for the HEADING test
test_heading_setpoint1 : [0.0, 0.0, 0.4, 0.0]
@@ -15,14 +15,21 @@ 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, 3.14]
+test_all_setpoint2 : [-0.3, 0.0, 0.7, 1.57]
+
+# Parameters for flying in a circle
+test_circle_radius : 0.40
+test_circle_seconds_per_rev : 3.0
+test_circle_height : 0.4
+test_circle_pirouette_per_rev : 0.0
+test_circle_time_to_reach_start : 0.7
# 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
+multiplier_vertical_max : 1.7
# 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 4a0648e7cc32a2de9e9f8a2845ae3da4b5011f38..09bf7db145ee1701a7805768084d7f8f80c29db3 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
@@ -187,6 +187,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+ // UPDATE THE SETPOINT FOR FLYING IN A CIRCLE - IF ACTIVE
+ if (isActive_fly_test_circle)
+ {
+ update_setpoint_for_test_circle();
+ }
+
+
// CARRY OUT THE CONTROLLER COMPUTATIONS
calculateControlOutput_viaLQR(current_bodyFrameError,request,response);
@@ -235,6 +242,12 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform HORIZONTAL test");
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
switch (test_horizontal_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -275,6 +288,12 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform VERTICAL test");
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
switch (test_vertical_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -315,6 +334,12 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform HEADING test");
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
switch (test_heading_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -355,6 +380,12 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Display what test will be activated
ROS_INFO("[TUNING CONTROLLER] Received message to perform ALL test");
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
switch (test_all_currentpoint)
{
// Currently at setpoint 1 => change to setpoint 2
@@ -388,6 +419,25 @@ void activateTestCallback(const std_msgs::Int32& msg)
break;
}
+ // Test FLY A CIRCLE
+ case 5:
+ {
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to FLY IN A CIRCLE");
+
+ // Check that not already flying a circle
+ if (!isActive_fly_test_circle)
+ {
+ // Set the boolean to make the circle active
+ isActive_fly_test_circle = true;
+
+ // (Re-)set the tick counter to zero
+ test_circle_ticks_since_start = 0;
+
+ }
+ break;
+ }
+
// Handle the exception
default:
@@ -399,15 +449,57 @@ void activateTestCallback(const std_msgs::Int32& msg)
}
}
+
+void update_setpoint_for_test_circle()
+{
+ // Compute the time since start
+ float time_since_start = float(test_circle_ticks_since_start) / vicon_frequency;
+
+ // Allow time to reach the start point
+ if (time_since_start < test_circle_time_to_reach_start)
+ {
+ // Set the initial setpoint
+ setpoint[0] = test_circle_radius;
+ setpoint[1] = 0.0;
+ setpoint[2] = test_circle_height;
+ setpoint[3] = 0.0;
+ }
+ else if (time_since_start < (2.2*test_circle_seconds_per_rev) )
+ {
+ // Compute the current point in the period
+ float omega_t = ((time_since_start-test_circle_time_to_reach_start)/test_circle_seconds_per_rev) * (2*PI);
+ // Set the current setpoint on the circle
+ setpoint[0] = test_circle_radius * cos(omega_t);
+ setpoint[1] = test_circle_radius * sin(omega_t);
+ setpoint[2] = test_circle_height;
+ setpoint[3] = test_circle_pirouette_per_rev * omega_t;
+ }
+ else
+ {
+ // Turn of the flz circle mode
+ isActive_fly_test_circle = false;
+ // Return to the center
+ setpoint[0] = 0.0;
+ setpoint[1] = 0.0;
+ setpoint[2] = test_circle_height;
+ setpoint[3] = 0.0;
+ }
+
+ // Increment the tick counter
+ test_circle_ticks_since_start++;
+}
+
+
+
// 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 );
}
@@ -417,10 +509,10 @@ 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 );
}
@@ -430,10 +522,10 @@ 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 );
}
@@ -1285,6 +1377,12 @@ 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);
+ // Parameters for flying in a circle
+ test_circle_radius = getParameterFloat(nodeHandle_for_TuningController, "test_circle_radius");
+ test_circle_seconds_per_rev = getParameterFloat(nodeHandle_for_TuningController, "test_circle_seconds_per_rev");
+ test_circle_height = getParameterFloat(nodeHandle_for_TuningController, "test_circle_height");
+ test_circle_time_to_reach_start = getParameterFloat(nodeHandle_for_TuningController, "test_circle_time_to_reach_start");
+ test_circle_pirouette_per_rev = getParameterFloat(nodeHandle_for_TuningController, "test_circle_pirouette_per_rev");
// Multipliers for the HORIZONTAL contorller
multiplier_horizontal_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_horizontal_min");