Commit 2fcc3700 authored by beuchatp's avatar beuchatp
Browse files

Added demo controller, renamed custom to student, and remove old files

parent 5a91a85c
......@@ -134,7 +134,6 @@ add_message_files(
CrazyflieDB.msg
#----------------------------------------------------------------------
DebugMsg.msg
CustomControllerYAML.msg
CustomButton.msg
)
......@@ -238,12 +237,9 @@ include_directories(
add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
add_executable(PPSClient src/PPSClient.cpp)
add_executable(SafeControllerService src/SafeControllerService.cpp)
add_executable(CustomControllerService src/CustomControllerService.cpp)
add_executable(DemoControllerService src/DemoControllerService.cpp)
add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/ParameterService.cpp)
add_executable(CircleControllerService src/CircleControllerService.cpp)
add_executable(FollowCrazyflieService src/FollowCrazyflieService.cpp)
add_executable(FollowN_1Service src/FollowN_1Service.cpp)
......@@ -287,12 +283,9 @@ qt5_use_modules(student_GUI Widgets)
add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DemoControllerService 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})
add_dependencies(CircleControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(FollowCrazyflieService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(FollowN_1Service d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# GUI-- dependencies
add_dependencies(my_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -324,18 +317,12 @@ target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
target_link_libraries(CircleControllerService ${catkin_LIBRARIES})
target_link_libraries(FollowCrazyflieService ${catkin_LIBRARIES})
target_link_libraries(FollowN_1Service ${catkin_LIBRARIES})
# GUI -- link libraries
target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
target_link_libraries(my_GUI Qt5::Svg)
......
<launch>
<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
</node>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
</node>
<node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" />
<!-- When we have a GUI, this has to be adapted and included -->
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node>
</launch>
<launch>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<param name="studentID" value="$(optenv ROS_NAMESPACE)" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<node pkg="d_fall_pps" name="FollowN_1Service" output="screen" type="FollowN_1Service">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<!-- When we have a GUI, this has to be adapted and included -->
<node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node>
<!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
</launch>
......@@ -24,7 +24,7 @@
<param name="type" type="str" value="agent" />
<param name="agentID" value="$(optenv ROS_NAMESPACE)" />
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
<rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" ns="CustomController" />
<rosparam command="load" file="$(find d_fall_pps)/param/DemoController.yaml" ns="CustomController" />
</node>
<!-- AGENT GUI (aka. the "student GUI") -->
......
<launch>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<param name="studentID" value="$(optenv ROS_NAMESPACE)" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<node pkg="d_fall_pps" name="CircleControllerService" output="screen" type="CircleControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
</launch>
<launch>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<param name="studentID" value="$(optenv ROS_NAMESPACE)" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<node pkg="d_fall_pps" name="FollowCrazyflieService" output="screen" type="FollowCrazyflieService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
</launch>
......@@ -15,7 +15,7 @@
<param name="type" type="str" value="coordinator" />
<param name="agentID" value="0" />
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
<rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" ns="CustomController" />
<rosparam command="load" file="$(find d_fall_pps)/param/DemoController.yaml" ns="CustomController" />
</node>
</launch>
float64 mass
float64 control_frequency
float64[] motorPoly
bool shouldPerformConvertIntoBodyFrame
bool shouldPublishCurrent_xyz_yaw
bool shouldFollowAnotherAgent
uint32[] follow_in_a_line_agentIDs
......@@ -2,7 +2,7 @@ safeController: "SafeControllerService/RateController"
customController: "CustomControllerService/CustomController"
strictSafety: true
angleMargin: 0.6
battery_threshold_while_flying: 2.9 # in V
battery_threshold_while_motors_off: 3.34 # in V
battery_polling_period: 100 # in ms
battery_threshold_while_flying: 2.8 # in V
battery_threshold_while_motors_off: 3.30 # in V
battery_polling_period: 100 # in ms
# Mass of the crazyflie
mass : 31
# 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]
# Boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame : true
# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
shouldPublishCurrent_xyz_yaw : false
# Boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
# an attempt to follow the "my_current_xyz_yaw_topic" from another agent
shouldFollowAnotherAgent : false
# The order in which agents should follow eachother
# > This parameter is a vector of integers that specifies agent ID's
# > The order of the agent ID's is the ordering of the line formation
# > i.e., the first ID is the leader, the second ID should follow the first ID, and so on
follow_in_a_line_agentIDs : [1, 2, 3]
# 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 to use, defined as:
# 1 - LQR controller based on the state vector: [position,velocity,angles]
# 2 - LQR controller based on the state vector: [position,velocity]
controller_type : 1
# The LQR Controller parameters for "mode = 1"
gainMatrixRollRate : [ 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00]
gainMatrixPitchRate : [ 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84]
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00]
# The LQR Controller parameters for "mode = 2"
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]
gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.31, 0.00, 0.00, 0.14]
#equlibrium offset
mass : 29
mass : 30
#quadratic motor regression equation (a0, a1, a2)
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
......@@ -7,7 +7,7 @@ motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
gainMatrixThrust: [0, 0, 0.20195826, 0, 0, 0.08362477, 0, 0, 0]
#kalman filter
filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
......@@ -18,9 +18,9 @@ defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
landingDistance : 0.05
durationTakeOff : 1.5
durationLanding : 2
landingDistance : -0.05
durationTakeOff : 1.0
durationLanding : 2.0
# Liner Trayectory following parameters
distanceThreshold : 0.5
// Copyright (C) 2017, ETH Zurich, D-ITET, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// 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:
// Alternate controller that flies the quadcopter in a circle.
//
// ----------------------------------------------------------------------------------
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <ros/package.h>
#include "std_msgs/Float32.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#define PI 3.1415926535
#define RATE_CONTROLLER 7
using namespace d_fall_pps;
std::vector<float> ffThrust(4);
std::vector<float> feedforwardMotor(4);
std::vector<float> motorPoly(3);
std::vector<float> gainMatrixRoll(9);
std::vector<float> gainMatrixPitch(9);
std::vector<float> gainMatrixYaw(9);
std::vector<float> gainMatrixThrust(9);
//K_infinite of feedback
std::vector<float> filterGain(6);
//only for velocity calculation
std::vector<float> estimatorMatrix(2);
float prevEstimate[9];
float saturationThrust;
CrazyflieData previousLocation;
//circle stuff
float currentTime;
const float OMEGA = 0.25*2*PI;
const float RADIUS = 0.25;
//point publisher for FollowCrazyflieService
ros::Publisher followPublisher;
void loadParameterFloatVector(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");
}
}
void loadParameters(ros::NodeHandle& nodeHandle) {
loadParameterFloatVector(nodeHandle, "feedforwardMotor", feedforwardMotor, 4);
loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
for(int i = 0; i < 4; ++i) {
ffThrust[i] = motorPoly[2] * feedforwardMotor[i] * feedforwardMotor[i] + motorPoly[1] * feedforwardMotor[i] + motorPoly[0];
}
saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
}
float computeMotorPolyBackward(float thrust) {
return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
}
//Kalman
void estimateState(Controller::Request &request, float (&est)[9]) {
// attitude
est[6] = request.ownCrazyflie.roll;
est[7] = request.ownCrazyflie.pitch;
est[8] = request.ownCrazyflie.yaw;
//velocity & filtering
float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
float k_x[6]; //filterGain times state
k_x[0] = request.ownCrazyflie.x * filterGain[0];
k_x[1] = request.ownCrazyflie.y * filterGain[1];
k_x[2] = request.ownCrazyflie.z * filterGain[2];
k_x[3] = request.ownCrazyflie.x * filterGain[3];
k_x[4] = request.ownCrazyflie.y * filterGain[4];
k_x[5] = request.ownCrazyflie.z * filterGain[5];
est[0] = ahat_x[0] + k_x[0];
est[1] = ahat_x[1] + k_x[1];
est[2] = ahat_x[2] + k_x[2];
est[3] = ahat_x[3] + k_x[3];
est[4] = ahat_x[4] + k_x[4];
est[5] = ahat_x[5] + k_x[5];
memcpy(prevEstimate, est, 9 * sizeof(float));
}
void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
float sinYaw = sin(yaw_measured);
float cosYaw = cos(yaw_measured);
state[0] = est[0] * cosYaw + est[1] * sinYaw;
state[1] = -est[0] * sinYaw + est[1] * cosYaw;
state[2] = est[2];
state[3] = est[3] * cosYaw + est[4] * sinYaw;
state[4] = -est[3] * sinYaw + est[4] * cosYaw;
state[5] = est[5];
state[6] = est[6];
state[7] = est[7];
state[8] = est[8];
}
void calculateCircle(Setpoint &circlePoint){
circlePoint.x = 0;
circlePoint.y = RADIUS*sin(OMEGA*currentTime);
circlePoint.z = RADIUS*cos(OMEGA*currentTime);
circlePoint.yaw = 0;//OMEGA*currentTime;
}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
CrazyflieData vicon = request.ownCrazyflie;
currentTime += request.ownCrazyflie.acquiringTime;
//publish setpoint for FollowController of another student group
Setpoint pubSetpoint;
pubSetpoint.x = request.ownCrazyflie.x;
pubSetpoint.y = request.ownCrazyflie.y;
pubSetpoint.z = request.ownCrazyflie.z;
pubSetpoint.yaw = request.ownCrazyflie.yaw;
followPublisher.publish(pubSetpoint);
//actual Circle Controller stuff follows
Setpoint circlePoint;
calculateCircle(circlePoint);
float yaw_measured = request.ownCrazyflie.yaw;
//move coordinate system to make setpoint origin
request.ownCrazyflie.x -= circlePoint.x;
request.ownCrazyflie.y -= circlePoint.y;
request.ownCrazyflie.z -= circlePoint.z;
float yaw = request.ownCrazyflie.yaw - circlePoint.yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
request.ownCrazyflie.yaw = yaw;
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est);
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
convertIntoBodyFrame(est, state, yaw_measured);
//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;
if(thrustIntermediate > saturationThrust)
thrustIntermediate = saturationThrust;
else if(thrustIntermediate < -saturationThrust)
thrustIntermediate = -saturationThrust;
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + ffThrust[0]);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + ffThrust[1]);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + ffThrust[2]);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + ffThrust[3]);
response.controlOutput.onboardControllerType = RATE_CONTROLLER;
previousLocation = request.ownCrazyflie;
return true;
}
int main(int argc, char* argv[]) {
ros::init(argc, argv, "CircleControllerService");
currentTime = 0;
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
//publisher for Follow Controller of another student group
followPublisher = nodeHandle.advertise<Setpoint>("FollowTopic", 1);
ros::ServiceServer service = nodeHandle.advertiseService("CircleController", calculateControlOutput);
ROS_INFO("CircleControllerService ready");
ros::spin();
return 0;
}
This diff is collapsed.
// Copyright (C) 2017, ETH Zurich, D-ITET, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// 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:
// Controller that follows the position of another Crazyflie
//
// ----------------------------------------------------------------------------------
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <ros/package.h>
#include "std_msgs/Float32.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#define PI 3.1415926535
#define RATE_CONTROLLER 7