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 db9556d9d87be7be44416e055c8b76977eae6e15..90417e6dd6eff95799949df68c2136128dc4247e 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
@@ -211,11 +211,6 @@ void MainWindow::updateBatteryVoltage(float battery_voltage)
// Need to take voltage, display it and transform it to percentage
int percentage = (int) fromVoltageToPercent(m_battery_voltage);
- if(percentage != ui->battery_bar->value())
- {
- // ui->battery_bar->setValue(percentage);
- }
-
QString qstr = "Raw voltage: ";
qstr.append(QString::number(battery_voltage, 'f', 2));
ui->raw_voltage->setText(qstr);
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 031cb6a45e12d377e494a3a2cc40a6e520dbfd74..ebd5a7cf428e24d2d2001605babb0084ca83a417 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
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>854</width>
- <height>569</height>
+ <height>616</height>
</rect>
</property>
<property name="windowTitle">
@@ -42,10 +42,10 @@
<string/>
</property>
<layout class="QGridLayout" name="gridLayout_10">
- <item row="1" column="0">
+ <item row="2" column="0">
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
- <number>1</number>
+ <number>0</number>
</property>
<widget class="QWidget" name="tab_3">
<attribute name="title">
@@ -625,39 +625,48 @@
</widget>
</widget>
</item>
- <item row="1" column="1">
+ <item row="2" column="1">
<widget class="QGroupBox" name="groupBox_4">
<property name="title">
<string/>
</property>
- <layout class="QGridLayout" name="gridLayout_6">
- <item row="1" column="1">
- <widget class="QLabel" name="flying_state_label">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QPushButton" name="motors_OFF_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
- <string>FlyingState</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Motors OFF</string>
</property>
</widget>
</item>
- <item row="2" column="0" colspan="3">
- <widget class="QPushButton" name="land_button">
+ <item>
+ <widget class="QPushButton" name="take_off_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="text">
- <string>Land</string>
+ <string>Take Off</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="take_off_button">
+ <item>
+ <widget class="QPushButton" name="land_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="text">
- <string>Take Off</string>
+ <string>Land</string>
</property>
</widget>
</item>
@@ -666,44 +675,47 @@
</item>
<item row="0" column="0">
<widget class="QGroupBox" name="groupBox">
+ <property name="font">
+ <font>
+ <pointsize>18</pointsize>
+ </font>
+ </property>
<property name="title">
<string>StudentID # connected to CF #</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="0" column="3">
+ <item row="0" column="2">
<widget class="QLabel" name="raw_voltage">
- <property name="text">
- <string>Raw voltage: </string>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QProgressBar" name="battery_bar">
- <property name="value">
- <number>24</number>
+ <property name="font">
+ <font>
+ <pointsize>18</pointsize>
+ <italic>false</italic>
+ </font>
</property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="motors_OFF_button">
<property name="text">
- <string>Motors OFF</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Battery level</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ <string>Raw voltage: </string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="flying_state_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>FlyingState</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
@@ -715,7 +727,7 @@
<x>0</x>
<y>0</y>
<width>854</width>
- <height>19</height>
+ <height>25</height>
</rect>
</property>
</widget>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
index 3b064d2339f42557e51b032dbc9c7d731163b999..8c884a793bb87ea42747eb446fbad356a6ba4559 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
@@ -1,10 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-09-05T11:57:14. -->
+<!-- Written by QtCreator 3.5.1, 2017-09-08T16:35:09. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
- <value type="QByteArray">{72576140-2426-4e8d-b4f8-00ed8021ee7f}</value>
+ <value type="QByteArray">{1400dcd4-82c6-466c-a808-34f7a3d4fe21}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
@@ -40,7 +40,6 @@
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
- <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
@@ -59,21 +58,22 @@
<data>
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.57.gcc_64_kit</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{051621a5-413a-4a38-907c-a6d036ac454e}</value>
<value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
+ <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop-Debug</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
<value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
@@ -126,7 +126,7 @@
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Release</value>
+ <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop-Release</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
@@ -134,6 +134,7 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
<value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
<value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
@@ -185,67 +186,7 @@
<value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
<value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
</valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.2">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Profile</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">true</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Profile</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">3</value>
+ <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">2</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
@@ -261,11 +202,6 @@
<value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
- <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
- <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
- <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
- <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
- <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
@@ -304,13 +240,12 @@
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">studentGUI</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro</value>
- <value type="bool" key="QmakeProjectManager.QmakeRunConfiguration.UseLibrarySearchPath">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro</value>
<value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
<value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">studentGUI.pro</value>
<value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseTerminal">false</value>
<value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
<value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
<value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index b275c4ce0a3ab979ea6611fadf68d230afc83313..f2336c54249b824af613e3e9c0c4b5fcf5fd2da1 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1,3 +1,3 @@
5,PPS_CF02,0/8/2M,1,-0.401205,0.729533,-0.2,0.289141,1.25551,2
-6,PPS_CF08,0/56/2M,2,-0.997887,-0.285942,-0.2,0.00653149,0.214912,2
-4,PPS_CF05,0/32/2M,0,-1.1263,0.101319,-0.2,-0.403531,0.665187,2
+6,PPS_CF08,0/56/2M,2,-0.997887,-0.285942,-0.2,0.00653145,0.214912,2
+4,PPS_CF05,0/32/2M,0,-0.85884,0.248234,-0.2,-0.136071,0.812101,2
diff --git a/pps_ws/src/d_fall_pps/param/CustomController.yaml b/pps_ws/src/d_fall_pps/param/CustomController.yaml
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..8f59bc1539c5c13ded4c40c38cf1c0583dc5a2a1 100644
--- a/pps_ws/src/d_fall_pps/param/CustomController.yaml
+++ b/pps_ws/src/d_fall_pps/param/CustomController.yaml
@@ -0,0 +1,8 @@
+# mass of the crazyflie
+mass : 30
+
+# frequency of the controller, in hertz
+control_frequency : 200
+
+#quadratic motor regression equation (a0, a1, a2)
+motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index ab612385896e372eadf69fcff7c3c79ce7ec7946..406e0a21977b2fa62fc9cbc82bffd3e000e9a8a9 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -21,15 +21,23 @@
#define ANGLE_MODE 1
#define MOTOR_MODE 2
-//constants that you most probably need for your controller to work properly
-//see: 2015-08 - Forster - System ID of Crazyflie 2.pdf Chapter 3.3.1: Input Command → Thrust
-const float FEEDFORWARD_MOTOR[4] = {37000, 37000, 37000, 37000};
-const float MOTOR_REGRESSION_POLYNOMIAL[3] = {5.484560e-4, 1.032633e-6, 2.130295e-11};
-const float SATURATION_THRUST = MOTOR_REGRESSION_POLYNOMIAL[2] * 12000 * 12000 + MOTOR_REGRESSION_POLYNOMIAL[1] * 12000 + MOTOR_REGRESSION_POLYNOMIAL[1];
-
//namespacing the package
using namespace d_fall_pps;
+// variables for controller
+float cf_mass; //crazyflie mass in grams
+std::vector<float> motorPoly(3);
+float control_frequency;
+float gravity_force;
+
+CrazyflieData previous_location;
+
+const float gainMatrixRoll[9] = {0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0};
+const float gainMatrixPitch[9] = {1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0};
+const float gainMatrixYaw[9] = {0, 0, 0, 0, 0, 0, 0, 0, 2.843099534};
+const float gainMatrixThrust[9] = {0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0};
+
+
// load parameters from corresponding YAML file
@@ -42,9 +50,34 @@ void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std
}
}
+float getFloatParameter(ros::NodeHandle& nodeHandle, std::string name)
+{
+
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
void loadCustomParameters(ros::NodeHandle& nodeHandle)
{
// here we load the parameters that are in the CustomController.yaml
+
+ cf_mass = getFloatParameter(nodeHandle, "mass");
+ control_frequency = getFloatParameter(nodeHandle, "control_frequency");
+ loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
+
+ // compute things that we will need after from these parameters
+
+ // force that we need to counteract gravity (mg)
+ gravity_force = cf_mass * 9.81/1000; // in N
+}
+
+float computeMotorPolyBackward(float thrust) {
+ return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
}
@@ -52,7 +85,8 @@ void loadCustomParameters(ros::NodeHandle& nodeHandle)
//-est- is an array with the estimated values : x,y,z,vx,vy,vz,roll,pitch,yaw
//-estBody- is an EMPTY array which will then contain the values in the body frame used by the crazyflie
//-yaw_measured- is the value that came from Vicon
-void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured) {
+void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured)
+{
float sinYaw = sin(yaw_measured);
float cosYaw = cos(yaw_measured);
@@ -70,8 +104,6 @@ void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured) {
}
-
-
/* --- the data students can work with ---
-request- contains data provided by Vicon. Check d_fall_pps/msg/ViconData.msg what it includes.
-response- is where you have to write your calculated data into.
@@ -91,29 +123,55 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ********* do your calculations here *********
//Tip: create functions that you call here to keep you code cleaner
+ ROS_INFO("custom controller loop");
+ // calculate the velocity based in the derivative of the position
- ROS_INFO("custom controller loop");
+ float est[9];
+
+ est[0] = request.ownCrazyflie.x;
+ est[1] = request.ownCrazyflie.y;
+ est[2] = request.ownCrazyflie.z;
+ est[3] = (request.ownCrazyflie.x - previous_location.x) * control_frequency;
+ est[4] = (request.ownCrazyflie.y - previous_location.y) * control_frequency;
+ est[5] = (request.ownCrazyflie.z - previous_location.z) * control_frequency;
+ est[6] = request.ownCrazyflie.roll;
+ est[7] = request.ownCrazyflie.pitch;
+ est[8] = request.ownCrazyflie.yaw;
+ float state[9];
+ convertIntoBodyFrame(est, state, request.ownCrazyflie.yaw);
- //for students to set the newly calculated commands for the controller
- response.controlOutput.roll = 0;
- response.controlOutput.pitch = 0;
- response.controlOutput.yaw = 0; //in [rad] --> will be converted to degree in CrazyRadio.py before sending to Crazyflie
- response.controlOutput.motorCmd1 = 0;
- response.controlOutput.motorCmd2 = 0;
- response.controlOutput.motorCmd3 = 0;
- response.controlOutput.motorCmd4 = 0;
+ // calculate feedback
+ float outRoll = 0;
+ float outPitch = 0;
+ float outYaw = 0;
+ float thrustIntermediate = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ outRoll -= gainMatrixRoll[i] * state[i];
+ outPitch -= gainMatrixPitch[i] * state[i];
+ outYaw -= gainMatrixYaw[i] * state[i];
+ thrustIntermediate -= gainMatrixThrust[i] * state[i];
+ }
+ response.controlOutput.roll = outRoll;
+ response.controlOutput.pitch = outPitch;
+ response.controlOutput.yaw = outYaw;
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
/*choosing the Crazyflie onBoard controller type.
it can either be Motor, Rate or Angle based */
- response.controlOutput.onboardControllerType = MOTOR_MODE;
- //response.controlOutput.onboardControllerType = RATE_MODE;
- //response.controlOutput.onboardControllerType = ANGLE_MODE;
+ // response.controlOutput.onboardControllerType = MOTOR_MODE;
+ response.controlOutput.onboardControllerType = RATE_MODE;
+ // response.controlOutput.onboardControllerType = ANGLE_MODE;
+ previous_location = request.ownCrazyflie; // we have already used previous location, update it
return true;
}
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index b04cc185d88d6e83e567cb84591a9976b6ef791c..c9afacc494d04f3abcdb32ac2ad294d68ce8158c 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -124,7 +124,6 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
est[5] = ahat_x[5] + k_x[5];
memcpy(prevEstimate, est, 9 * sizeof(float));
-
}