diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index ea93a9fdffd97cc5736e86fb8e93052040542245..065f356282490ed3a1ae91a0e8d40d6a16dfe2f5 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -270,6 +270,7 @@ add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
+add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
@@ -323,6 +324,7 @@ add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${cat
add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(MpcControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(RemoteControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(TuningControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -364,6 +366,7 @@ else()
target_link_libraries(MpcControllerService ${catkin_LIBRARIES})
endif()
target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
+target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
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 e8480b3a54822766677e0baef0763626425bc38d..cea18ddcd5dc1c8098df34e4ed5c4211beff7000 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
@@ -52,6 +52,7 @@
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
// Commands for CrazyRadio
@@ -72,6 +73,7 @@
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
#define CMD_USE_REMOTE_CONTROLLER 5
+#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
@@ -93,12 +95,14 @@
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
+#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
+#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
// Universal constants
#define PI 3.141592653589
@@ -142,6 +146,7 @@ private slots:
void on_load_student_yaml_button_clicked();
void on_load_mpc_yaml_button_clicked();
void on_load_remote_yaml_button_clicked();
+ void on_load_tuning_yaml_button_clicked();
// # Enable controllers
void on_enable_safe_controller_clicked();
@@ -149,6 +154,7 @@ private slots:
void on_enable_student_controller_clicked();
void on_enable_mpc_controller_clicked();
void on_enable_remote_controller_clicked();
+ void on_enable_tuning_controller_clicked();
@@ -162,6 +168,15 @@ private slots:
void on_remote_activate_button_clicked();
void on_remote_deactivate_button_clicked();
+ // Buttons within the TUNING controller tab
+ void on_tuning_test_horizontal_button_clicked();
+ void on_tuning_test_vertical_button_clicked();
+ void on_tuning_test_heading_button_clicked();
+ void on_tuning_test_all_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);
+
private:
@@ -179,6 +194,8 @@ private:
ros::Timer m_timer_yaml_file_for_student_controller;
ros::Timer m_timer_yaml_file_for_mpc_controller;
ros::Timer m_timer_yaml_file_for_remote_controller;
+ ros::Timer m_timer_yaml_file_for_tuning_controller;
+
int m_student_id;
CrazyflieContext m_context;
@@ -210,6 +227,7 @@ private:
// > For the MPC Controller SETPOINTS
ros::Publisher mpcSetpointPublisher;
ros::Subscriber mpcSetpointSubscriber;
+
// > For the Remote Controller subscribe action
ros::Publisher remoteSubscribePublisher;
// > For the Remote Controller activate action
@@ -219,6 +237,13 @@ private:
// > For the Remote Control setpoint
ros::Subscriber remoteControlSetpointSubscriber;
+ // > For the TUNING CONTROLLER "test" button publisher
+ ros::Publisher tuningActivateTestPublisher;
+ // > For the TUNING CONTOLLER "gain" sliders
+ ros::Publisher tuningHorizontalGainPublisher;
+ ros::Publisher tuningVerticalGainPublisher;
+ ros::Publisher tuningHeadingGainPublisher;
+
ros::Publisher PPSClientStudentCustomButtonPublisher;
@@ -265,6 +290,7 @@ private:
void studentYamlFileTimerCallback(const ros::TimerEvent&);
void mpcYamlFileTimerCallback(const ros::TimerEvent&);
void remoteYamlFileTimerCallback(const ros::TimerEvent&);
+ void tuningYamlFileTimerCallback(const ros::TimerEvent&);
@@ -290,6 +316,7 @@ private:
void highlightStudentControllerTab();
void highlightMpcControllerTab();
void highlightRemoteControllerTab();
+ void highlightTuningControllerTab();
bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
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 dcb30009b620cb1614a2e229c7e75f7d4d060a34..afedd1f4546fb4f3e0e8bacd7ba91f3c068e7261 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
@@ -89,6 +89,15 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
remoteControlSetpointSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteControlSetpoint", 1, &MainWindow::remoteControlSetpointCallback, this);;
+ // > For the TUNING CONTROLLER "test" button publisher
+ tuningActivateTestPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/ActivateTest", 1);
+ // > For the TUNING CONTOLLER "gain" sliders
+ tuningHorizontalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HorizontalGain", 1);
+ tuningVerticalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/VerticalGain", 1);
+ tuningHeadingGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HeadingGain", 1);
+
+
+
// subscribers
crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
@@ -208,6 +217,7 @@ void MainWindow::highlightSafeControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightDemoControllerTab()
{
@@ -216,6 +226,7 @@ void MainWindow::highlightDemoControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightStudentControllerTab()
{
@@ -224,6 +235,7 @@ void MainWindow::highlightStudentControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightMpcControllerTab()
{
@@ -232,6 +244,7 @@ void MainWindow::highlightMpcControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightRemoteControllerTab()
{
@@ -240,6 +253,16 @@ void MainWindow::highlightRemoteControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::green);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
+}
+void MainWindow::highlightTuningControllerTab()
+{
+ ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::green);
}
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
@@ -266,6 +289,8 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
break;
case REMOTE_CONTROLLER:
highlightRemoteControllerTab();
+ case TUNING_CONTROLLER:
+ highlightTuningControllerTab();
break;
default:
break;
@@ -868,6 +893,36 @@ void MainWindow::remoteYamlFileTimerCallback(const ros::TimerEvent&)
+void MainWindow::on_load_tuning_yaml_button_clicked()
+{
+ // Set the "load tuning yaml" button to be disabled
+ ui->load_tuning_yaml_button->setEnabled(false);
+
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the tuning controller
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_TUNING_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("[STUDENT GUI] Request load of tuning controller YAML published");
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ ros::NodeHandle nodeHandle("~");
+ m_timer_yaml_file_for_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
+}
+
+void MainWindow::tuningYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load tuning yaml" button again
+ ui->load_tuning_yaml_button->setEnabled(true);
+}
+
+
+
void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
{
// Extract from the "msg" for which controller the YAML
@@ -955,6 +1010,21 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
break;
+ case LOAD_YAML_TUNING_CONTROLLER_AGENT:
+ case LOAD_YAML_TUNING_CONTROLLER_COORDINATOR:
+ // Set the "load tuning yaml" button to be disabled
+ ui->load_tuning_yaml_button->setEnabled(false);
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ m_timer_yaml_file_for_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
+
+ break;
+
default:
ROS_INFO("Unknown 'all controllers to load yaml' command, thus nothing will be disabled");
break;
@@ -1001,6 +1071,13 @@ void MainWindow::on_enable_remote_controller_clicked()
this->PPSClientCommandPublisher.publish(msg);
}
+void MainWindow::on_enable_tuning_controller_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_TUNING_CONTROLLER;
+ this->PPSClientCommandPublisher.publish(msg);
+}
+
// # Custom command buttons
@@ -1034,6 +1111,60 @@ void MainWindow::on_customButton_3_clicked()
+Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
+{
+ Setpoint corrected_setpoint;
+ corrected_setpoint = setpoint;
+
+ float x_size = context.localArea.xmax - context.localArea.xmin;
+ float y_size = context.localArea.ymax - context.localArea.ymin;
+ float z_size = context.localArea.zmax - context.localArea.zmin;
+
+ if(setpoint.x > x_size/2)
+ corrected_setpoint.x = x_size/2;
+ if(setpoint.y > y_size/2)
+ corrected_setpoint.y = y_size/2;
+ if(setpoint.z > z_size)
+ corrected_setpoint.z = z_size;
+
+ if(setpoint.x < -x_size/2)
+ corrected_setpoint.x = -x_size/2;
+ if(setpoint.y < -y_size/2)
+ corrected_setpoint.y = -y_size/2;
+ if(setpoint.z < 0)
+ corrected_setpoint.z = 0;
+
+ return corrected_setpoint;
+}
+
+bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
+{
+
+ float x_size = context.localArea.xmax - context.localArea.xmin;
+ float y_size = context.localArea.ymax - context.localArea.ymin;
+ float z_size = context.localArea.zmax - context.localArea.zmin;
+ //position check
+ if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
+ ROS_INFO_STREAM("x outside safety box");
+ return false;
+ }
+ if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
+ ROS_INFO_STREAM("y outside safety box");
+ return false;
+ }
+ if((setpoint.z < 0) or (setpoint.z > z_size)) {
+ ROS_INFO_STREAM("z outside safety box");
+ return false;
+ }
+
+ return true;
+}
+
+
+
+
+
+
// # Custom buttons for the REMOTE controller service
@@ -1127,52 +1258,75 @@ void MainWindow::remoteControlSetpointCallback(const CrazyflieData& setpointData
+// TUNING CONTROLLER TAB
+void MainWindow::on_tuning_test_horizontal_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 1;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
-Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
+void MainWindow::on_tuning_test_vertical_button_clicked()
{
- Setpoint corrected_setpoint;
- corrected_setpoint = setpoint;
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 2;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
- float x_size = context.localArea.xmax - context.localArea.xmin;
- float y_size = context.localArea.ymax - context.localArea.ymin;
- float z_size = context.localArea.zmax - context.localArea.zmin;
+void MainWindow::on_tuning_test_heading_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 3;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
- if(setpoint.x > x_size/2)
- corrected_setpoint.x = x_size/2;
- if(setpoint.y > y_size/2)
- corrected_setpoint.y = y_size/2;
- if(setpoint.z > z_size)
- corrected_setpoint.z = z_size;
+void MainWindow::on_tuning_test_all_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 4;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
- if(setpoint.x < -x_size/2)
- corrected_setpoint.x = -x_size/2;
- if(setpoint.y < -y_size/2)
- corrected_setpoint.y = -y_size/2;
- if(setpoint.z < 0)
- corrected_setpoint.z = 0;
+void MainWindow::on_tuning_slider_horizontal_valueChanged(int value)
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = value;
+ // Publish the message
+ this->tuningHorizontalGainPublisher.publish(msg);
+}
- return corrected_setpoint;
+void MainWindow::on_tuning_slider_vertical_valueChanged(int value)
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = value;
+ // Publish the message
+ this->tuningVerticalGainPublisher.publish(msg);
}
-bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
+void MainWindow::on_tuning_slider_heading_valueChanged(int value)
{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = value;
+ // Publish the message
+ this->tuningHeadingGainPublisher.publish(msg);
+}
- float x_size = context.localArea.xmax - context.localArea.xmin;
- float y_size = context.localArea.ymax - context.localArea.ymin;
- float z_size = context.localArea.zmax - context.localArea.zmin;
- //position check
- if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
- ROS_INFO_STREAM("x outside safety box");
- return false;
- }
- if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
- ROS_INFO_STREAM("y outside safety box");
- return false;
- }
- if((setpoint.z < 0) or (setpoint.z > z_size)) {
- ROS_INFO_STREAM("z outside safety box");
- return false;
- }
- return true;
-}
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 7513dcb809cac1e95ef34c7b436380b82ca8954c..58644b6da549fd7fe991aea8d3f30aa3db945871 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
@@ -369,7 +369,7 @@
</font>
</property>
<property name="currentIndex">
- <number>4</number>
+ <number>5</number>
</property>
<property name="usesScrollButtons">
<bool>true</bool>
@@ -2811,6 +2811,397 @@
</item>
</layout>
</widget>
+ <widget class="QWidget" name="tab">
+ <attribute name="title">
+ <string>Tuning</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_16">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_15">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_45">
+ <property name="text">
+ <string>Vertical Controller Gain</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="Line" name="line_13">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="Line" name="line_15">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="0">
+ <spacer name="verticalSpacer_4">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Minimum</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>10</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="1" column="1">
+ <widget class="QPushButton" name="tuning_test_horizontal_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>160</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Test</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="Line" name="line_16">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <widget class="QLabel" name="label_46">
+ <property name="text">
+ <string>Heading Controller Gain</string>
+ </property>
+ </widget>
+ </item>
+ <item row="12" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QPushButton" name="tuning_test_all_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>400</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Test All</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <widget class="Line" name="line_14">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <spacer name="verticalSpacer_5">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Minimum</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>5</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="9" column="1">
+ <widget class="QPushButton" name="tuning_test_heading_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>160</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Test</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_37">
+ <property name="text">
+ <string>Horizontal Controller Gain</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_39">
+ <property name="text">
+ <string>Min</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSlider" name="tuning_slider_vertical">
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="tracking">
+ <bool>false</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksAbove</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_40">
+ <property name="text">
+ <string>Max</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="1">
+ <widget class="QPushButton" name="tuning_test_vertical_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>160</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Test</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <spacer name="verticalSpacer_6">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Minimum</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>5</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="9" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_41">
+ <property name="text">
+ <string>Min</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSlider" name="tuning_slider_heading">
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="tracking">
+ <bool>false</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksAbove</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_42">
+ <property name="text">
+ <string>Max</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_43">
+ <property name="text">
+ <string>Min</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSlider" name="tuning_slider_horizontal">
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="tracking">
+ <bool>false</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksAbove</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_44">
+ <property name="text">
+ <string>Max</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="10" column="0">
+ <widget class="Line" name="line_17">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="1">
+ <widget class="Line" name="line_18">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
<item>
@@ -2834,6 +3225,30 @@
<property name="bottomMargin">
<number>6</number>
</property>
+ <item row="6" column="2">
+ <widget class="QPushButton" name="load_mpc_yaml_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>MPC</string>
+ </property>
+ </widget>
+ </item>
<item row="7" column="2">
<widget class="QPushButton" name="load_remote_yaml_button">
<property name="sizePolicy">
@@ -3001,7 +3416,7 @@
</property>
</widget>
</item>
- <item row="8" column="0">
+ <item row="9" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
@@ -3109,36 +3524,36 @@
</property>
</widget>
</item>
- <item row="3" column="1">
- <widget class="Line" name="line_3">
+ <item row="4" column="1">
+ <widget class="Line" name="line_4">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="Line" name="line_6">
+ <item row="3" column="1">
+ <widget class="Line" name="line_3">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="Line" name="line_4">
+ <item row="1" column="1">
+ <widget class="Line" name="line_6">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
- <item row="8" column="1">
- <widget class="Line" name="line_5">
+ <item row="0" column="1">
+ <widget class="Line" name="line_7">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="Line" name="line_7">
+ <item row="9" column="1">
+ <widget class="Line" name="line_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
@@ -3168,14 +3583,20 @@
</property>
</widget>
</item>
- <item row="6" column="2">
- <widget class="QPushButton" name="load_mpc_yaml_button">
+ <item row="7" column="0">
+ <widget class="QPushButton" name="enable_remote_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
<property name="maximumSize">
<size>
<width>16777215</width>
@@ -3188,24 +3609,42 @@
</font>
</property>
<property name="text">
- <string>MPC</string>
+ <string>Remote</string>
</property>
</widget>
</item>
- <item row="7" column="0">
- <widget class="QPushButton" name="enable_remote_controller">
+ <item row="8" column="0">
+ <widget class="QPushButton" name="enable_tuning_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="minimumSize">
+ <property name="maximumSize">
<size>
- <width>0</width>
- <height>0</height>
+ <width>16777215</width>
+ <height>50</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Tuning</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="2">
+ <widget class="QPushButton" name="load_tuning_yaml_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="maximumSize">
<size>
<width>16777215</width>
@@ -3218,7 +3657,7 @@
</font>
</property>
<property name="text">
- <string>Remote</string>
+ <string>Tuning</string>
</property>
</widget>
</item>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
index 3a37faf9d1bdafe6e65e9d559b1c4c13696ca09e..82d1be478e0ec7c4f5ee39f13646dfac63520507 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -88,6 +88,7 @@
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
// The constants that "command" changes in the
// operation state of this agent
@@ -96,6 +97,8 @@
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
#define CMD_USE_REMOTE_CONTROLLER 5
+#define CMD_USE_TUNING_CONTROLLER 6
+
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
@@ -153,6 +156,8 @@ ros::ServiceClient studentController;
ros::ServiceClient mpcController;
// The Remote controller specified in the ClientConfig.yaml
ros::ServiceClient remoteController;
+// The Tuning controller specified in the ClientConfig.yaml
+ros::ServiceClient tuningController;
//values for safteyCheck
@@ -297,6 +302,7 @@ void loadDemoController();
void loadStudentController();
void loadMpcController();
void loadRemoteController();
+void loadTuningController();
void sendMessageUsingController(int controller);
void setInstantController(int controller);
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
index 7edcebdd65bd87a0b7253d5b9850fb584035f36a..12ef8b42b6c02b580a75bc1864b8002b3eaf7bc1 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
@@ -40,12 +40,14 @@
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
+#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
+#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
// For sending commands to the controller node informing
@@ -60,9 +62,11 @@
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
#define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT 5
+#define FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT 6
#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
-#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
\ No newline at end of file
+#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
+#define FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR 16
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..71068b101959deb1df1df31f02921e0a1429c7c6
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -0,0 +1,452 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+//the generated structs from the msg-files have to be included
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+#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"
+
+#include <std_msgs/Int32.h>
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
+#define PI 3.1415926535
+
+// These constants define the modes that can be used for controller the Crazyflie 2.0,
+// the constants defined here need to be in agreement with those defined in the
+// firmware running on the Crazyflie 2.0.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+
+#define CF_COMMAND_TYPE_MOTOR 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+//
+// LQR_MODE_MOTOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands per motor thrusts
+//
+// LQR_MODE_ACTUATOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands actuators of total force and bodz torques
+//
+// LQR_MODE_RATE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_MODE_ANGLE LQR controller based on the state vector:
+// [position,velocity]
+//
+//#define CONTROLLER_MODE_LQR_MOTOR 1
+//#define CONTROLLER_MODE_LQR_ACTUATOR 2
+#define CONTROLLER_MODE_LQR_RATE 3 // (DEFAULT)
+#define CONTROLLER_MODE_LQR_ANGLE 4
+#define CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED 5
+//#define CONTROLLER_MODE_ANGLE_RESPONSE_TEST 6
+
+
+// These constants define the method used for estimating the Inertial
+// frame state.
+// All methods are run at all times, this flag indicates which estimate
+// is passed onto the controller.
+// The following is a short description about each mode:
+//
+// ESTIMATOR_METHOD_FINITE_DIFFERENCE
+// Takes the poisition and angles directly as measured,
+// and estimates the velocities as a finite different to the
+// previous measurement
+//
+// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
+// Uses a 2nd order random walk estimator independently for
+// each of (x,y,z,roll,pitch,yaw)
+//
+// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
+// Uses the model of the quad-rotor and the previous inputs
+//
+#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
+#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
+#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+// LQR_RATE_MODE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_ANGLE_MODE LQR controller based on the state vector:
+// [position,velocity]
+//
+#define LQR_RATE_MODE 1 // (DEFAULT)
+#define LQR_ANGLE_MODE 2
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+// ******************************************************************************* //
+// VARIABLES SPECIFIC TO THE TUNING CONTROL FEATURE
+
+// Setpoint for the HORIZONTAL test
+int test_horizontal_currentpoint = 1;
+std::vector<float> test_horizontal_setpoint1 (4,0.0);
+std::vector<float> test_horizontal_setpoint2 (4,0.0);
+
+// Setpoint for the VERTICAL test
+int test_vertical_currentpoint = 1;
+std::vector<float> test_vertical_setpoint1 (4,0.0);
+std::vector<float> test_vertical_setpoint2 (4,0.0);
+
+// Setpoint for the HEADING test
+int test_heading_currentpoint = 1;
+std::vector<float> test_heading_setpoint1 (4,0.0);
+std::vector<float> test_heading_setpoint2 (4,0.0);
+
+// Setpoint for the ALL test
+int test_all_currentpoint = 1;
+std::vector<float> test_all_setpoint1 (4,0.0);
+std::vector<float> test_all_setpoint2 (4,0.0);
+
+
+// LQR Gain matrix for tracking the angle commands from the Crazyflie
+std::vector<float> gainMatrixRollRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixPitchRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixYawRate_forRemoteControl (3,0.0);
+
+// ******************************************************************************* //
+
+
+
+// VARIABLES FOR SOME "ALMOST CONSTANTS"
+// > Mass of the Crazyflie quad-rotor, in [grams]
+float cf_mass;
+// > Coefficients of the 16-bit command to thrust conversion
+std::vector<float> motorPoly(3);
+// The weight of the Crazyflie in [Newtons], i.e., mg
+float gravity_force;
+// One quarter of the "gravity_force"
+float gravity_force_quarter;
+
+
+
+
+// VARIABLES FOR THE CONTROLLER
+
+// Frequency at which the controller is running
+float vicon_frequency;
+
+// Frequency at which the controller is running
+float control_frequency;
+
+
+// > The setpoints for (x,y,z) position and yaw angle, in that order
+float setpoint[4] = {0.0,0.0,0.4,0.0};
+
+
+
+
+// The controller type to run in the "calculateControlOutput" function
+int controller_mode = CONTROLLER_MODE_LQR_RATE;
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
+std::vector<float> gainMatrixRollRate (9,0.0);
+std::vector<float> gainMatrixPitchRate (9,0.0);
+std::vector<float> gainMatrixYawRate (9,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
+std::vector<float> gainMatrixThrust_SixStateVector (6,0.0);
+std::vector<float> gainMatrixRollAngle (6,0.0);
+std::vector<float> gainMatrixPitchAngle (6,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
+std::vector<float> gainMatrixThrust_SixStateVector_50Hz (6,0.0);
+std::vector<float> gainMatrixRollAngle_50Hz (6,0.0);
+std::vector<float> gainMatrixPitchAngle_50Hz (6,0.0);
+
+std::vector<float> gainMatrixRollRate_Nested (3,0.0);
+std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
+std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+
+int lqr_angleRateNested_counter = 4;
+float lqr_angleRateNested_prev_thrustAdjustment = 0.0;
+float lqr_angleRateNested_prev_rollAngle = 0.0;
+float lqr_angleRateNested_prev_pitchAngle = 0.0;
+float lqr_angleRateNested_prev_yawAngle = 0.0;
+
+
+// The 16-bit command limits
+float cmd_sixteenbit_min;
+float cmd_sixteenbit_max;
+
+
+// VARIABLES FOR THE ESTIMATOR
+
+// Frequency at which the controller is running
+float estimator_frequency;
+
+// > A flag for which estimator to use:
+int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+// > The current state interial estimate,
+// for use by the controller
+float current_stateInertialEstimate[12];
+
+// > The measurement of the Crazyflie at the "current" time step,
+// to avoid confusion
+float current_xzy_rpy_measurement[6];
+
+// > The measurement of the Crazyflie at the "previous" time step,
+// used for computing finite difference velocities
+float previous_xzy_rpy_measurement[6];
+
+// > The full 12 state estimate maintained by the finite
+// difference state estimator
+float stateInterialEstimate_viaFiniteDifference[12];
+
+// > The full 12 state estimate maintained by the point mass
+// kalman filter state estimator
+float stateInterialEstimate_viaPointMassKalmanFilter[12];
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+// > For the (roll,pitch,yaw) angles
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+
+
+
+// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
+ros::Publisher debugPublisher;
+
+
+// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
+
+// Boolean whether to execute the convert into body frame function
+bool shouldPerformConvertIntoBodyFrame = false;
+
+
+// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+
+// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
+// POSITION
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+
+
+
+// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
+// The "CrazyflieData" type used for the "request" variable is a
+// structure as defined in the file "CrazyflieData.msg" which has the following
+// properties:
+// string crazyflieName The name given to the Crazyflie in the Vicon software
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+// > The function that is called to "start" all estimation and control computations
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// > The various functions that implement an LQR controller
+void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+
+// ESTIMATOR COMPUTATIONS
+void performEstimatorUpdate_forStateInterial(Controller::Request &request);
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+// PUBLISHING OF THE DEBUG MESSAGE
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
+
+
+// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// SETPOINT CHANGE CALLBACK
+void setpointCallback(const Setpoint& newSetpoint);
+
+
+
+// LOAD PARAMETERS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+
+
+
+// ******************************************************************************* //
+// FUNCTIONS SPECIFIC TO THE TUNING CONTROL FEATURE
+
+// ACTIVATE THE TESTS
+void activateTestCallback(const std_msgs::Int32& msg);
+// CHANGE THE GAINS
+void horizontalGainCallback(const std_msgs::Int32& msg);
+void verticalGainCallback(const std_msgs::Int32& msg);
+void headingGainCallback(const std_msgs::Int32& msg);
+
+// ******************************************************************************* //
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 73d16618f0566630facb936297788c32a286f929..c525b10152e175be591335bd3bf900015a440cbe 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -3,6 +3,7 @@ demoController: "DemoControllerService/DemoController"
studentController: "StudentControllerService/StudentController"
mpcController: "MpcControllerService/MpcController"
remoteController: "RemoteControllerService/RemoteController"
+tuningController: "TuningControllerService/TuningController"
strictSafety: false
angleMargin: 0.6
diff --git a/pps_ws/src/d_fall_pps/param/TuningController.yaml b/pps_ws/src/d_fall_pps/param/TuningController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c03384f7e9a9d6b40326402bfdbea5d1b571be00
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/TuningController.yaml
@@ -0,0 +1,113 @@
+# ***************************************************************************** #
+# 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];
+
+# 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];
+
+# 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];
+
+# 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];
+
+# ***************************************************************************** #
+
+
+
+# Mass of the crazyflie
+mass : 29
+
+# Frequency of the controller, in hertz
+vicon_frequency : 200
+control_frequency : 200
+
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# Boolean for whether to execute the convert into body frame function
+shouldPerformConvertIntoBodyFrame : true
+
+# Boolean indiciating whether the "Debug Message" of this agent should be published or not
+shouldPublishDebugMessage : false
+
+# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+shouldDisplayDebugInfo : false
+
+
+# A flag for which controller mode to use, defined as:
+# 1 CONTROLLER_MODE_LQR_MOTOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands per motor thrusts
+# 2 CONTROLLER_MODE_LQR_ACTUATOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands actuators of total force and bodz torques
+# 3 CONTROLLER_MODE_LQR_RATE
+# - LQR controller based on the state vector: [position,velocity,angles]
+# 4 CONTROLLER_MODE_LQR_ANGLE
+# - LQR controller based on the state vector: [position,velocity]
+# 5 CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED
+# - LQR Nested angle and rate controller
+# 6 CONTROLLER_MODE_ANGLE_RESPONSE_TEST
+# - Swaps between pitch set-points to test angle set-point response time
+# i.e., this controller test the assumption that "the inner loop is infinitely fast"
+#
+controller_mode : 3
+
+
+# A flag for which estimator to use, defined as:
+# 1 - Finite Different Method,
+# Takes the poisition and angles directly as measured,
+# and estimates the velocities as a finite different to the
+# previous measurement
+# 2 - Point Mass Per Dimension Method
+# Uses a 2nd order random walk estimator independently for
+# each of (x,y,z,roll,pitch,yaw)
+# 3 - Quad-rotor Model Based Method
+# Uses the model of the quad-rotor and the previous inputs
+estimator_method : 1
+
+
+# The LQR Controller parameters for "mode = 3"
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
+gainMatrixRollRate : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00]
+gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00]
+gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+
+# The LQR Controller parameters for "mode = 4"
+gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25]
+gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
+gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
+
+# The LQR Controller parameters for "mode = 5"
+gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
+gainMatrixRollAngle_50Hz : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
+gainMatrixPitchAngle_50Hz : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
+
+gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
+gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
+gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
+
+
+# The max and minimum thrust for a 16-bit command
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 60000
+
+
+# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+# > For the (x,y,z) position
+PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
+PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
+# > For the (roll,pitch,yaw) angles
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index e14630c8370ff649fa194c33f2ec846ae9b39684..bebe2e1251ee4b29263b3733aa9807f563798d5d 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -301,6 +301,9 @@ void viconCallback(const ViconData& viconData)
case REMOTE_CONTROLLER:
success = remoteController.call(controllerCall);
break;
+ case TUNING_CONTROLLER:
+ success = tuningController.call(controllerCall);
+ break;
default:
ROS_ERROR("[PPS CLIENT] the current controller was not recognised");
break;
@@ -457,6 +460,11 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
setControllerUsed(REMOTE_CONTROLLER);
break;
+ case CMD_USE_TUNING_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_TUNING_CONTROLLER Command received");
+ setControllerUsed(TUNING_CONTROLLER);
+ break;
+
case CMD_CRAZYFLY_TAKE_OFF:
if(flying_state == STATE_MOTORS_OFF)
{
@@ -832,6 +840,21 @@ void loadRemoteController()
ROS_INFO_STREAM("[PPS CLIENT] Loaded remote controller " << remoteController.getService());
}
+void loadTuningController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string tuningControllerName;
+ if(!nodeHandle.getParam("tuningController", tuningControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get tuning controller name");
+ return;
+ }
+
+ tuningController = ros::service::createClient<Controller>(tuningControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded tuning controller " << tuningController.getService());
+}
+
void sendMessageUsingController(int controller)
{
// send a message in topic for the studentGUI to read it
@@ -861,6 +884,9 @@ void setInstantController(int controller) //for right now, temporal use
case REMOTE_CONTROLLER:
loadRemoteController();
break;
+ case TUNING_CONTROLLER:
+ loadTuningController();
+ break;
default:
break;
}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
index f3faa81ade9e6632ab46fa4fca10ab11f18a0efa..2fd671074cab9aeb1183dafb56b120be14c1529d 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -150,6 +150,17 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// Re-load the parameters of the demo controller:
cmd = "rosparam load " + d_fall_pps_path + "/param/RemoteController.yaml " + m_base_namespace + "/RemoteController";
}
+ // ----------------------------------------
+ // FOR THE TUNING CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
+ {
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/TuningController.yaml " + m_base_namespace + "/TuningController";
+ }
else
{
// Let the user know that the command was not recognised
diff --git a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..815e664b01bf405c6cbfa6c533001c40d9fed648
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
@@ -0,0 +1,1605 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/TuningControllerService.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// 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
+// 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)
+//
+// The arument "request" is a structure provided to this service with the following two
+// properties:
+// request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is defined in the
+// file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the Vicon
+// motion capture system of the Crazyflie that is to be controlled by this service
+//
+// request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access to the
+// Vicon measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by this
+// service by this function, it has only the following property
+// response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is defined in
+// the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines" section at the
+// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
+// > In the PPS exercise we will only use the RATE_MODE.
+// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested from the
+// PID controllers running in the Crazyflie 2.0 firmware.
+// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
+// are the baseline motor commands requested from the Crazyflie, with the adjustment
+// for body rates being added on top of this in the firmware (i.e., as per the code
+// of the "distribute_power" function provided in exercise sheet 2).
+// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
+// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
+// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
+// thrust). To assist, teh following is an ASCII art of this convention:
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points our of the screen,
+// > This being a "top view" means tha the direction of positive thrust produced
+// by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+
+
+
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // THIS IS THE START OF THE "OUTER" CONTROL LOOP
+ // > i.e., this is the control loop run on this laptop
+ // > this function is called at the frequency specified
+ // > this function performs all estimation and control
+
+
+ // PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
+ // > After this function is complete the class variable
+ // "current_stateInertialEstimate" is updated and ready
+ // to be used for subsequent controller copmutations
+ performEstimatorUpdate_forStateInterial(request);
+
+
+ // CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
+ // THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
+ // > Define a local array to fill in with the body frame error
+ float current_bodyFrameError[12];
+ // > Call the function to perform the conversion
+ convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+
+
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ calculateControlOutput_viaLQR(current_bodyFrameError,request,response);
+
+
+ // PUBLISH THE DEBUG MESSAGE (if required)
+ if (shouldPublishDebugMessage)
+ {
+ construct_and_publish_debug_message(request,response);
+ }
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE M M OOO TTTTT EEEEE
+// R R E MM MM O O T E
+// RRRR EEE M M M O O T EEE
+// R R E M M O O T E
+// R R EEEEE M M OOO T EEEEE
+//
+// CCCC OOO N N TTTTT RRRR OOO L
+// C O O NN N T R R O O L
+// C O O N N N T RRRR O O L
+// C O O N NN T R R O O L
+// CCCC OOO N N T R R OOO LLLLL
+// ----------------------------------------------------------------------------------
+
+
+// ACTIVATE THE TESTS
+void activateTestCallback(const std_msgs::Int32& msg)
+{
+
+ // Get the test index from the message
+ int test_index = msg.data;
+
+ switch (test_index)
+ {
+ // Test the HORIZONTAL controller
+ case 1:
+ {
+ switch (test_horizontal_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_horizontal_setpoint2[0];
+ setpoint[1] = test_horizontal_setpoint2[1];
+ setpoint[2] = test_horizontal_setpoint2[2];
+ setpoint[3] = test_horizontal_setpoint2[3];
+ test_horizontal_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_horizontal_setpoint1[0];
+ setpoint[1] = test_horizontal_setpoint1[1];
+ setpoint[2] = test_horizontal_setpoint1[2];
+ setpoint[3] = test_horizontal_setpoint1[3];
+ test_horizontal_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_horizontal_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+
+ // Test the VERTICAL controller
+ case 2:
+ {
+ switch (test_vertical_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_vertical_setpoint2[0];
+ setpoint[1] = test_vertical_setpoint2[1];
+ setpoint[2] = test_vertical_setpoint2[2];
+ setpoint[3] = test_vertical_setpoint2[3];
+ test_vertical_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_vertical_setpoint1[0];
+ setpoint[1] = test_vertical_setpoint1[1];
+ setpoint[2] = test_vertical_setpoint1[2];
+ setpoint[3] = test_vertical_setpoint1[3];
+ test_vertical_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_vertical_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+
+ // Test the HEADING controller
+ case 3:
+ {
+ switch (test_heading_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_heading_setpoint2[0];
+ setpoint[1] = test_heading_setpoint2[1];
+ setpoint[2] = test_heading_setpoint2[2];
+ setpoint[3] = test_heading_setpoint2[3];
+ test_heading_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_heading_setpoint1[0];
+ setpoint[1] = test_heading_setpoint1[1];
+ setpoint[2] = test_heading_setpoint1[2];
+ setpoint[3] = test_heading_setpoint1[3];
+ test_heading_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_heading_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+
+ // Test ALL the controllers
+ case 4:
+ {
+ switch (test_all_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_all_setpoint2[0];
+ setpoint[1] = test_all_setpoint2[1];
+ setpoint[2] = test_all_setpoint2[2];
+ setpoint[3] = test_all_setpoint2[3];
+ test_all_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_all_setpoint1[0];
+ setpoint[1] = test_all_setpoint1[1];
+ setpoint[2] = test_all_setpoint1[2];
+ setpoint[3] = test_all_setpoint1[3];
+ test_all_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_all_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_index' is not recognised.");
+ break;
+ }
+ }
+}
+
+// CHANGE THE GAIN FOR THE HORIZONTAL CONTROLLER
+void horizontalGainCallback(const std_msgs::Int32& msg)
+{
+
+}
+
+// CHANGE THE GAIN FOR THE VERTICAL CONTROLLER
+void verticalGainCallback(const std_msgs::Int32& msg)
+{
+
+}
+
+// CHANGE THE GAIN FOR THE HEADING CONTROLLER
+void headingGainCallback(const std_msgs::Int32& msg)
+{
+
+}
+
+
+
+
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
+// E S T I MM MM A A T I O O NN N
+// EEE SSS T I M M M A A T I O O N N N
+// E S T I M M AAAAA T I O O N NN
+// EEEEE SSSS T III M M A A T III OOO N N
+// ------------------------------------------------------------------------------
+void performEstimatorUpdate_forStateInterial(Controller::Request &request)
+{
+
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+
+
+ // RUN THE FINITE DIFFERENCE ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+
+
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ }
+
+
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
+ previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
+ previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
+ previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
+ previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+{
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[12];
+ for(int i = 0; i < 12; ++i)
+ {
+ temp_PMKFstate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
+ stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
+ stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L QQQ RRRR
+// L Q Q R R
+// L Q Q RRRR
+// L Q Q R R
+// LLLLL QQ Q R R
+// ----------------------------------------------------------------------------------
+
+void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // SWITCH BETWEEN THE DIFFERENT LQR CONTROLLER MODES:
+ switch (controller_mode)
+ {
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_RATE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity]
+ case CONTROLLER_MODE_LQR_ANGLE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAngles(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAnglesRatesNested(stateErrorBody,request,response);
+ break;
+ }
+
+ default:
+ {
+ // Display that the "controller_mode" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'controller_mode' is not recognised.");
+ // Set everything in the response to zero
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ response.controlOutput.motorCmd1 = 0;
+ response.controlOutput.motorCmd2 = 0;
+ response.controlOutput.motorCmd3 = 0;
+ response.controlOutput.motorCmd4 = 0;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ break;
+ }
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 9; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > 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.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollAngle_forResponse = 0;
+ float pitchAngle_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
+ }
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollAngle_forResponse;
+ response.controlOutput.pitch = pitchAngle_forResponse;
+ response.controlOutput.yaw = setpoint[3];
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > 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.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+}
+
+
+
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Increment the counter variable
+ lqr_angleRateNested_counter++;
+
+ if (lqr_angleRateNested_counter > 4)
+ {
+ //ROS_INFO("Outer called");
+
+ // Reset the counter to 1
+ lqr_angleRateNested_counter = 1;
+
+ // PERFORM THE OUTER "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Reset the class variable to zero for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > body frame yaw angle,
+ // > total thrust adjustment.
+ // These will be requested from the "inner-loop" LQR controller below
+ lqr_angleRateNested_prev_rollAngle = 0;
+ lqr_angleRateNested_prev_pitchAngle = 0;
+ lqr_angleRateNested_prev_thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
+ }
+
+ // BODY FRAME Z CONTROLLER
+ //lqr_angleRateNested_prev_yawAngle = setpoint[3];
+ lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
+
+
+ }
+
+ //ROS_INFO("Inner called");
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateErrorBody[6] - lqr_angleRateNested_prev_rollAngle,
+ stateErrorBody[7] - lqr_angleRateNested_prev_pitchAngle,
+ lqr_angleRateNested_prev_yawAngle
+ };
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 4; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > 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.
+ float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
+{
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
+
+ // DEBUGGING CODE:
+ // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
+ // By fill this message with data and publishing it you can display the data in
+ // real time using rpt plots. Instructions for using rqt plots can be found on
+ // the wiki of the D-FaLL-System repository
+
+ // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
+ // (located in the "msg" folder) to see the full structure of this message.
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // Fill in the debugging message with any other data you would like to display
+ // in real time. For example, it might be useful to display the thrust
+ // adjustment computed by the z-altitude controller.
+ // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
+ // type "float64" that you can fill in with data you would like to plot in
+ // real-time.
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
+// R R O O T A A T E I NN N T O O
+// RRRR O O T A A T EEE I N N N T O O
+// R R O O T AAAAA T E I N NN T O O
+// R R OOO T A A T EEEEE III N N T OOO
+//
+// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
+// B B O O D D Y Y F R R A A MM MM E
+// BBBB O O D D Y FFF RRRR A A M M M EEE
+// B B O O D D Y F R R AAAAA M M E
+// BBBB OOO DDDD Y F R R A A M M EEEEE
+// ------------------------------------------------------------------------------
+
+// The arguments for this function are as follows:
+// stateInertial
+// This is an array of length 9 with the estimates the error of of the following values
+// relative to the sepcifed setpoint:
+// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
+// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
+// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
+//
+// stateBody
+// This is an empty array of length 9, this function should fill in all elements of this
+// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
+// position and (x,y) velocities are rotated into the body frame.
+//
+// yaw_measured
+// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
+// the Vicon motion capture system
+//
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured)
+{
+ if (shouldPerformConvertIntoBodyFrame)
+ {
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
+ stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
+ stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+ else
+ {
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0];
+ stateBody[1] = stateInertial[1];
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3];
+ stateBody[4] = stateInertial[4];
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+}
+
+
+
+
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12])
+{
+ // Store the current YAW in a local variable
+ float temp_stateInertial_yaw = stateInertial[8];
+
+ // Adjust the INERTIAL (x,y,z) position for the setpoint
+ stateInertial[0] = stateInertial[0] - setpoint[0];
+ stateInertial[1] = stateInertial[1] - setpoint[1];
+ stateInertial[2] = stateInertial[2] - setpoint[2];
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = stateInertial[8] - setpoint[3];
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, put the "yawError" into the "stateError" variable
+ stateInertial[8] = yawError;
+
+
+ if (yawError>(PI/6))
+ {
+ yawError = (PI/6);
+ }
+ else if (yawError<(-PI/6))
+ {
+ yawError = (-PI/6);
+ }
+
+ // CONVERSION INTO BODY FRAME
+ // Conver the state erorr from the Inertial frame into the Body frame
+ // > Note: the function "convertIntoBodyFrame" is implemented in this file
+ // and by default does not perform any conversion. The equations to convert
+ // the state error into the body frame should be implemented in that function
+ // for successful completion of the PPS exercise
+ convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command signal that generates the "thrust" force
+ float cmd = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd < cmd_sixteenbit_min)
+ {
+ cmd = 0;
+ }
+ else if (cmd > cmd_sixteenbit_max)
+ {
+ cmd = cmd_sixteenbit_max;
+ }
+
+ return cmd;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// GGG U U III CCCC A L L BBBB A CCCC K K
+// G G U U I C A A L L B B A A C K K
+// G U U I C A A L L BBBB A A C KKK
+// G G U U I C AAAAA L L B B AAAAA C K K
+// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void setpointCallback(const Setpoint& newSetpoint)
+{
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the and from where to fetch the YAML
+ // parameters
+ int controller_to_fetch_yaml = msg.data;
+
+ // Switch between fetching for the different controllers and from different locations
+ switch(controller_to_fetch_yaml)
+ {
+
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[TUNING CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[TUNING CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ 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("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters fetched from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the CustomController.yaml file
+
+ // Add the "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_TuningController(nodeHandle, "TuningController");
+
+
+ // ******************************************************************************* //
+ // PARAMETERS SPECIFIC TO THE TUNING CONTROL FEATURE
+
+ /// Setpoint for the HORIZONTAL test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_horizontal_setpoint1", test_horizontal_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_horizontal_setpoint2", test_horizontal_setpoint2, 4);
+
+ // Setpoint for the VERTICAL test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_vertical_setpoint1", test_vertical_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_vertical_setpoint2", test_vertical_setpoint2, 4);
+
+ // Setpoint for the HEADING test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_heading_setpoint1", test_heading_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_heading_setpoint2", test_heading_setpoint2, 4);
+
+ // Setpoint for the ALL test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint1", test_all_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint2", test_all_setpoint2, 4);
+
+ // // 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);
+
+ // ******************************************************************************* //
+
+
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_TuningController , "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
+ vicon_frequency = getParameterFloat(nodeHandle_for_TuningController, "vicon_frequency");
+
+ // > 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_TuningController, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_TuningController, "motorPoly", motorPoly, 3);
+
+ // > The boolean for whether to execute the convert into body frame function
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_TuningController, "shouldPerformConvertIntoBodyFrame");
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_TuningController, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_TuningController, "shouldDisplayDebugInfo");
+
+
+ // THE CONTROLLER GAINS:
+
+ // A flag for which controller to use:
+ controller_mode = getParameterInt( nodeHandle_for_TuningController , "controller_mode" );
+
+ // A flag for which estimator to use:
+ estimator_method = getParameterInt( nodeHandle_for_TuningController , "estimator_method" );
+
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE"
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+
+
+ // 16-BIT COMMAND LIMITS
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_TuningController, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_TuningController, "command_sixteenbit_max");
+
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ //ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: the fetched TuningController/mass = " << cf_mass);
+
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters loaded from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+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);
+ gravity_force_quarter = cf_mass * 9.81/(1000*4);
+
+ // 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);
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
+{
+ std::string val;
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "TuningControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "StudentControllerService" node
+ // > This should be something like: "/dfall/agentXXX"
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // the following line of code:
+ // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "studentID" to the "PPSClient"
+ // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
+ // to the "PPSClient" node within which this controller service is nested.
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("[TUNING CONTROLLER] Failed to get agentID from PPSClient");
+ }
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
+
+ // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
+ // for the parameter service that is running on the coordinate machine
+ // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
+ // is very important because it specifies that the name is global
+ namespace_to_coordinator_parameter_service = "/ParameterService";
+
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
+ //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("[TUNING CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[TUNING CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[TUNING CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // 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);
+
+ // *********************************************************************************
+
+
+
+ // Subscribe to the message that activates the tests
+ ros::Subscriber tuningActivateTestSubscriber = nodeHandle.subscribe("ActivateTest", 1, activateTestCallback);
+
+ // Subscribe to the message that changes the gains
+ ros::Subscriber tuningHorizontalGainSubscriber = nodeHandle.subscribe("HorizontalGain", 1, horizontalGainCallback);
+ ros::Subscriber tuningVerticalGainSubscriber = nodeHandle.subscribe("VerticalGain", 1, verticalGainCallback);
+ ros::Subscriber tuningHeadingGainSubscriber = nodeHandle.subscribe("HeadingGain", 1, headingGainCallback);
+
+
+
+ // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
+ // advertises under the name "DebugTopic" and is a message with the structure
+ // defined in the file "DebugMsg.msg" (located in the "msg" folder).
+ debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "Setpoint" topic and calls the class function
+ // "setpointCallback" each time a messaged is received on this topic and the message
+ // is passed as an input argument to the "setpointCallback" class function.
+ //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
+ // 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
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("TuningController", calculateControlOutput);
+
+ // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
+ // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
+ // "using namespace d_fall_pps;"
+ // in the "DEFINES" section at the top of this file.
+ ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+
+ // Print out some information to the user.
+ ROS_INFO("[TUNING CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}