more pictures in wiki

pps_wiki/workflow_for_students.md

@@ -89,7 +89,8 @@ important files that should be taken into account.
 
 * In `d_fall_pps/param`
 
-  * _ClientConfig.yaml_ <br>
+  * _ClientConfig.yaml_ <br><br>
+          <img src="./pics/client_config_yaml.png" style="width: 400px;"/> <br><br>
   This file needs to be changed to define names for the custom controller. **The safeController property shouldn't be changed!** <br>
 
       *Usage:*
@@ -168,12 +169,15 @@ important files that should be taken into account.
    that the file is already filled with some code. This code is a simple LQR
    controller that is offered as a template for the student to start developing
    their own controller. Change the file as you wish with your own controller
-   algorithm.
+   algorithm. The function partially shown below is the most important part of
+   our this file:<br><br>
+        <img src="./pics/custom_controller_src.png" style="width: 700px;"/> <br><br>
 
      In the template you can also see an example of how to use the
      `CustomController.yaml` to load parameters that you may want to change
-     without having to compile or restart the system.
-
+     without having to compile or restart the system. Here, as an example, we
+     pass some parameters that can be seen below:<br><br>
+        <img src="./pics/custom_controller_yaml.png" style="width: 400px;"/> <br><br>
 2. Go to `cd ~/work/D-FaLL-System/pps_ws` and write `catkin_make`.
 
 3. Once everything has compiled without errors, run the next launch file:

pps_ws/src/d_fall_pps/src/CustomControllerService.cpp

@@ -133,16 +133,18 @@ 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];
+    float est[9];               // vector for the estimation of the state
 
     est[0] = request.ownCrazyflie.x;
     est[1] = request.ownCrazyflie.y;
     est[2] = request.ownCrazyflie.z;
 
+    // estimate speed of crazyflie. Simplest way: discrete derivative
     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;
@@ -167,7 +169,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     	thrustIntermediate -= gainMatrixThrust[i] * state[i];
     }
 
-    ROS_INFO_STREAM("thrustIntermediate = " << thrustIntermediate);    
+    ROS_INFO_STREAM("thrustIntermediate = " << thrustIntermediate);
 
     response.controlOutput.roll = outRoll;
     response.controlOutput.pitch = outPitch;