Merge branch 'student_GUI' of...

Merge branch 'student_GUI' of https://gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System into student_GUI

pps_wiki/installation.md

@@ -28,6 +28,13 @@ Add a new line in the ``/etc/hosts`` file that links the teacher's IP with the d
 
 - USB Crazyradio: <br />
 To set up the crazyradio USB dongle just copy the rule files ``99-crazyflie.rules`` and ``99-crazyradio.rules`` to the directory ``/etc/udev/rules.d``.
+You also have to install the library pyusb:
+```
+sudo apt-get update
+sudo apt-get install python-pip
+sudo pip install pyusb
+
+```
 - Source scripts in ``.bashrc``: <br />
 You need to source the following scripts in the ``.bashrc`` file:
   - the ROS setup script: ``/opt/ros/kinetic/setup.bash``

pps_ws/src/d_fall_pps/src/CustomControllerService.cpp

@@ -133,6 +133,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     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];
 

pps_ws/src/d_fall_pps/src/SafeControllerService.cpp

@@ -174,6 +174,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     request.ownCrazyflie.y -= setpoint[1];
     request.ownCrazyflie.z -= setpoint[2];
     float yaw = request.ownCrazyflie.yaw - setpoint[3];
+
 	
     //bag.write("Offset", ros::Time::now(), request.ownCrazyflie);