Commit 78039869 authored by roangel's avatar roangel
Browse files

Added changes in student GUI and also custom controller

parent be5b4ef0
......@@ -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);
......
......@@ -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>
......
<?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>
......
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
# 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]
......@@ -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;
}
......
......@@ -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));
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment