diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index da5513edb5b4593113476311d444f1784eb253a7..58a69ae5b4a79f25e292b220b181b1a8db41a280 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -240,6 +240,7 @@ add_executable(CustomControllerService src/CustomControllerService.cpp)
add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(CircleControllerService src/CircleControllerService.cpp)
add_executable(FollowCrazyflieService src/FollowCrazyflieService.cpp)
+add_executable(FollowN_1Service src/FollowN_1Service.cpp)
@@ -286,6 +287,7 @@ add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catk
add_dependencies(CentralManagerService 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})
@@ -325,6 +327,8 @@ 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)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp
index de4ab8cbc8aa397cacbd5dc0b3f1a7bf00cba1d8..93f0950169b18aabe7c2eacc1b1969aa8c071363 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp
@@ -11,7 +11,7 @@ std::map<std::string, std::string> channel_LUT
{"cfThree", "0/72/2M"},
{"cfFour", "0/99/2M"},
{"PPS_CF01", "0/0/2M"},
- {"PPS_CF02", "0/8/2M"},
+ {"PPS_CF02", "0/8/2M/E7E7E7E702"},
{"PPS_CF03", "0/16/2M"},
{"PPS_CF04", "0/24/2M"},
{"PPS_CF05", "0/32/2M"},
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 60f8f8c9dd2e4816bee956076cf873f3ad76e651..979ff704a35518200995cddd2cc6b2dc4fd3b238 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
@@ -59,7 +59,6 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
-
// First get student ID
if(!nh_PPSClient.getParam("studentID", m_student_id))
{
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 8c884a793bb87ea42747eb446fbad356a6ba4559..d14c1b24d636e554a87253e2ae2b7242c0e8b1ae 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,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 3.5.1, 2017-09-08T16:35:09. -->
+<!-- Written by QtCreator 3.5.1, 2017-09-08T19:15:31. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
diff --git a/pps_ws/src/d_fall_pps/launch/FollowN_1.launch b/pps_ws/src/d_fall_pps/launch/FollowN_1.launch
new file mode 100644
index 0000000000000000000000000000000000000000..750e4db856d11fe383b10fa7bcbae7ce739563f3
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/launch/FollowN_1.launch
@@ -0,0 +1,28 @@
+<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>
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 4e69d2ac84fe673b27600ff0c8e26d9cb4f64b65..cc9e62d0fbc75c8980fdc6e2e1795fde9d3228f5 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,5 +1,5 @@
safeController: "SafeControllerService/RateController"
-customController: "CustomControllerService/CustomController"
+customController: "FollowN_1Service/FollowController"
strictSafety: true
angleMargin: 0.6
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index 54a5f7cbfe4c2951a9ee952c564da8784ecab111..7e90e6d4446ae7033550b141442619959ba77d63 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1,3 +1,2 @@
-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.00653145,0.214912,2
-4,PPS_CF05,0/32/2M,0,-1.02122,0.593954,-0.2,-0.298455,1.15782,2
+1,PPS_CF02,0/8/2M/E7E7E7E702,0,-1.38022,-0.405516,-0.2,-0.481301,0.485645,2
+2,PPS_CF04,0/24/2M,1,-0.412714,-0.376959,-0.2,0.423148,0.384735,2
diff --git a/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp b/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e097efdd9eee649825a1800f4cf7b2757c221ad
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp
@@ -0,0 +1,255 @@
+// Alternate controller that is expected to work.
+// Copyright (C) 2017 Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// This program 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.
+//
+// This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
+
+#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 0
+
+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];
+
+std::vector<float> setpoint(4);
+float saturationThrust;
+ros::Publisher followPublisher;
+ros::Subscriber followSubscriber;
+
+CrazyflieData previousLocation;
+
+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);
+
+ loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
+}
+
+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];
+}
+
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
+ CrazyflieData vicon = request.ownCrazyflie;
+
+ float yaw_measured = request.ownCrazyflie.yaw;
+
+ //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);
+
+ //move coordinate system to make setpoint origin
+ request.ownCrazyflie.x -= setpoint[0];
+ request.ownCrazyflie.y -= setpoint[1];
+ request.ownCrazyflie.z -= setpoint[2];
+ float yaw = request.ownCrazyflie.yaw - setpoint[3];
+
+ 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];
+ }
+
+ //INFORMATION: this ugly fix was needed for the older firmware
+ //outYaw *= 0.5;
+
+ 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;
+}
+
+//the setpoint is copied from the other crazyflie. This crazyflie should then make the same figure in its own area
+void followCallback(const Setpoint& newSetpoint) {
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+ ROS_INFO("received callback new setpoint from FollowN1Service/FollowTopic");
+}
+
+
+int main(int argc, char* argv[]) {
+ ros::init(argc, argv, "FollowN_1Service");
+
+ ros::NodeHandle nodeHandle("~");
+ loadParameters(nodeHandle);
+
+ int student_id;
+ std::string m_namespace = ros::this_node::getNamespace();
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+
+ if(!PPSClient_nodeHandle.getParam("studentID", student_id))
+ {
+ ROS_ERROR("Failed to get studentID from FollowN_1Service");
+ }
+
+ followPublisher = nodeHandle.advertise<Setpoint>("/" + std::to_string(student_id) + "/FollowN_1Service/FollowTopic", 1);
+
+ if(student_id != 1)
+ {
+ // subscribe to n-1 setpoint
+ followSubscriber = nodeHandle.subscribe("/" + std::to_string(student_id - 1) + "/FollowN_1Service/FollowTopic", 1, followCallback);
+ }
+ else
+ {
+ // what is the setpoint for 1? 0 0 0.5 0
+ setpoint[0] = 0;
+ setpoint[1] = 0;
+ setpoint[2] = 0.5;
+ setpoint[3] = 0;
+ }
+
+
+ ros::ServiceServer service = nodeHandle.advertiseService("FollowController", calculateControlOutput);
+ ROS_INFO("FollowN_1Service ready");
+
+ ros::spin();
+
+ return 0;
+}