diff --git a/conf/airframes/tudelft/bebop2_opticflow_sim.xml b/conf/airframes/tudelft/bebop2_opticflow_sim.xml
diff --git a/conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml b/conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml
diff --git a/conf/flight_plans/rotorcraft_guido_optitrack.xml b/conf/flight_plans/rotorcraft_guido_optitrack.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
 
-<flight_plan alt="1.5" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Optitrack (Delft)" security_height="0.3">
+<flight_plan alt="3.5" ground_alt="3" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Optitrack (Delft)" security_height="0.3">
   <header>
 #include "autopilot.h"
 </header>

diff --git a/conf/messages.xml b/conf/messages.xml
diff --git a/conf/modules/optical_flow_landing.xml b/conf/modules/optical_flow_landing.xml
@@ -31,6 +31,7 @@
       <define name="IGAIN_HORIZONTAL_FACTOR" value="0.0001" description="Factor multiplied with I gain for horizontal control"/>
       <define name="ROLL_TRIM" value="0.0" description="Roll trim in degrees"/>
       <define name="PITCH_TRIM" value="0.0" description="Pitch trim in degrees"/>
+      <define name="FRONT_DIV_THRESHOLD" value="0.3" description="Threshold on front divergence for stopping."/>
       <!-- TODO: add more explanations of settings below -->
     </section>
   </doc>
@@ -72,6 +73,7 @@
         <dl_setting var="of_landing_ctrl.omega_LR" min="-60.0" step="1.0" max="60.0" module="ctrl/optical_flow_landing" shortname="omega_LR" param="OFL_OMEGA_LR"/>
         <dl_setting var="of_landing_ctrl.omega_FB" min="-60.0" step="1.0" max="60.0" module="ctrl/optical_flow_landing" shortname="omega_FB" param="OFL_OMEGA_FB"/>
 	<dl_setting var="of_landing_ctrl.active_motion" min="0" step="1" max="2" values="none|flow|angle" module="ctrl/optical_flow_landing" shortname="active_motion" param="OFL_ACTIVE_MOTION"/>
+	<dl_setting var="of_landing_ctrl.front_div_threshold" min="0.0" step="0.01" max="5.0" module="ctrl/optical_flow_landing" shortname="front_div_threshold" param="OFL_FRONT_DIV_THRESHOLD"/>
       </dl_settings>
     </dl_settings>
   </settings>

diff --git a/conf/userconf/tudelft/guido_conf.xml b/conf/userconf/tudelft/guido_conf.xml
@@ -21,17 +21,6 @@
    settings_modules="modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
    gui_color="red"
   />
-  <aircraft
-   name="Bebop2_opticflow_sim"
-   ac_id="4"
-   airframe="airframes/tudelft/bebop2_opticflow_sim.xml"
-   radio="radios/dummy.xml"
-   telemetry="telemetry/default_rotorcraft.xml"
-   flight_plan="flight_plans/tudelft/rotorcraft_guido_optitrack.xml"
-   settings="settings/rotorcraft_basic.xml [settings/control/stabilization_indi.xml] settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/air_data.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
-   gui_color="blue"
-  />
   <aircraft
    name="Bebop2_undistort_front"
    ac_id="6"

diff --git a/conf/userconf/tudelft/guido_control_panel.xml b/conf/userconf/tudelft/guido_control_panel.xml
@@ -119,7 +119,11 @@
         <arg flag="-ac 9999" constant="5"/>
       </program>
       <program name="Messages (Python)"/>
-      <program name="Real-time Plotter"/>
+      <program name="Real-time Plotter">
+        <arg flag="-u" constant="0.05"/>
+        <arg flag="-m" constant="1000"/>
+        <arg flag=""/>
+      </program>
       <program name="Messages"/>
     </session>
   </section>

diff --git a/sw/airborne/boards/bebop/mt9v117.c b/sw/airborne/boards/bebop/mt9v117.c
@@ -37,7 +37,6 @@
 #include <linux/videodev2.h>
 #include <linux/v4l2-mediabus.h>
 
-#include "generated/airframe.h"
 #ifdef BOARD_DISCO
 #include "boards/disco.h"
 #else

diff --git a/sw/airborne/boards/bebop/mt9v117.h b/sw/airborne/boards/bebop/mt9v117.h
@@ -29,7 +29,8 @@
 
 #include "std.h"
 #include "mcu_periph/i2c.h"
-//#include "generated/airframe.h"
+// Camera parameters are defined in sections
+#include "generated/airframe.h"
 
 #ifndef MT9V117_TARGET_FPS
 #define MT9V117_TARGET_FPS 0

diff --git a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
@@ -36,6 +36,7 @@
 #include "lib/vision/image.h"
 #include "lib/v4l/v4l2.h"
 #include "opticflow/opticflow_calculator.h"
+#include "generated/airframe.h"
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>

diff --git a/sw/airborne/modules/computer_vision/textons.c b/sw/airborne/modules/computer_vision/textons.c
@@ -34,6 +34,7 @@
 #include "modules/computer_vision/cv.h"
 #include "modules/computer_vision/textons.h"
 #include "mcu_periph/sys_time.h"
+#include "generated/airframe.h"
 
 float ** **dictionary;
 uint32_t learned_samples = 0;

diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.c b/sw/airborne/modules/ctrl/optical_flow_landing.c
@@ -200,6 +200,10 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID)
 #define OFL_ACTIVE_MOTION 0
 #endif
 
+#ifndef OFL_FRONT_DIV_THRESHOLD
+#define OFL_FRONT_DIV_THRESHOLD 0.3
+#endif
+
 // Normally, horizontal control is done via sending angle commands to INDI, so 0 (false)
 // When this is 1 (true),a change in angle will be commanded instead.
 #define HORIZONTAL_RATE_CONTROL 0
@@ -218,11 +222,15 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID)
 
 // variables retained between module calls
 
+
+float old_flow_time;
+
 // horizontal loop:
 float optical_flow_x;
 float optical_flow_y;
 float lp_flow_x;
 float lp_flow_y;
+float lp_divergence_front;
 float sum_roll_error;
 float sum_pitch_error;
 
@@ -240,6 +248,8 @@ float previous_cov_err;
 int32_t thrust_set;
 float divergence_setpoint;
 
+float divergence_front;
+
 // *********************************
 // include and define stuff for SSL:
 // *********************************
@@ -290,7 +300,7 @@ static void send_divergence(struct transport_tx *trans, struct link_device *dev)
 {
   pprz_msg_send_DIVERGENCE(trans, dev, AC_ID,
                            &(of_landing_ctrl.divergence), &divergence_vision_dt, &normalized_thrust,
-                           &cov_div, &pstate, &pused, &(of_landing_ctrl.agl));
+                           &cov_div, &pstate, &pused, &(of_landing_ctrl.agl), &lp_divergence_front);
 }
 
 /// Function definitions
@@ -371,6 +381,8 @@ void vertical_ctrl_module_init(void)
   of_landing_ctrl.omega_LR = OFL_OMEGA_LR;
   of_landing_ctrl.active_motion = OFL_ACTIVE_MOTION;
 
+  of_landing_ctrl.front_div_threshold = OFL_FRONT_DIV_THRESHOLD;
+
   int i;
   if (of_landing_ctrl.use_bias) {
     weights = (float *)calloc(n_textons + 1, sizeof(float));
@@ -423,6 +435,11 @@ void vertical_ctrl_module_init(void)
 
   lp_flow_x = 0.0f;
   lp_flow_y = 0.0f;
+  lp_divergence_front = 0.0f;
+
+  old_flow_time = get_sys_time_float();
+
+  divergence_front = 0.0f;
 }
 
 /**
@@ -484,6 +501,7 @@ static void reset_all_vars(void)
 
   lp_flow_x = 0.0f;
   lp_flow_y = 0.0f;
+  lp_divergence_front = 0.0f;
 }
 
 /**
@@ -899,8 +917,16 @@ void vertical_ctrl_module_run(bool in_flight)
               optical_flow_x;
   lp_flow_y = of_landing_ctrl.lp_factor_prediction * lp_flow_y + (1.0f - of_landing_ctrl.lp_factor_prediction) *
               optical_flow_y;
+  lp_divergence_front = of_landing_ctrl.lp_factor_prediction * lp_divergence_front + (1.0f - of_landing_ctrl.lp_factor_prediction) *
+              divergence_front;
+
 
 
+  if(lp_divergence_front > of_landing_ctrl.front_div_threshold) {
+      // Stop moving in the longitudinal direction:
+      of_landing_ctrl.omega_FB = 0.0f;
+  }
+
   if (of_landing_ctrl.active_motion == 1) {
     // Active motion through varying ventral flow commands
     float period_factor = 0.2;
@@ -1109,6 +1135,28 @@ void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp,
     divergence_vision = size_divergence;
     //printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision);
     vision_time = ((float)stamp) / 1e6;
+
+
+    // checking fps and newness of images:
+    /*float new_flow_time = get_sys_time_float();
+    float dt = (new_flow_time - old_flow_time);
+     if (dt > 0) {
+      float fps_flow = 1.0f / dt;
+      printf("FPS flow bottom cam in OF landing = %f, optical_flow_x = %f\n", fps_flow, optical_flow_x);
+    }
+    old_flow_time = new_flow_time; */
+  }
+  else {
+
+      float new_flow_time = get_sys_time_float();
+      float dt_flow_front = new_flow_time - old_flow_time;
+      if (dt_flow_front > 0) {
+	//float fps_flow = 1.0f / dt_flow_front;
+	//printf("FPS flow front cam in OF landing = %f\n", fps_flow);
+	old_flow_time = new_flow_time;
+
+	divergence_front = size_divergence / dt_flow_front;
+      }
   }
 }
 

diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.h b/sw/airborne/modules/ctrl/optical_flow_landing.h
@@ -94,6 +94,7 @@ struct OpticalFlowLanding {
   float omega_LR;   ///< Set point for the left-right ventral flow
   float omega_FB;   ///< Set point for the front-back ventral flow
   uint32_t active_motion; ///< Method for actively inducing motion
+  float front_div_threshold; ///< Threshold when the front div gets above this value, it will command a horizontal stop.
 };
 
 extern struct OpticalFlowLanding of_landing_ctrl;

diff --git a/sw/airborne/modules/ins/ins_flow.c b/sw/airborne/modules/ins/ins_flow.c
@@ -947,21 +947,28 @@ void ins_flow_update(void)
     F[i][i] = 1.0f;
   }
 
-  if(CONSTANT_ALT_FILTER) {
-    F[OF_V_IND][OF_ANGLE_IND] = dt*(g/(cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])));
-    F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f;
-    if(OF_TWO_DIM) {
-	F[OF_VX_IND][OF_THETA_IND] = dt*(g/(cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])));
+  if (CONSTANT_ALT_FILTER) {
+    F[OF_V_IND][OF_ANGLE_IND] = dt * (g / (cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND])));
+    F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
+    if (OF_TWO_DIM) {
+      F[OF_VX_IND][OF_THETA_IND] = dt * (g / (cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND])));
+    }
+  } else {
+    F[OF_V_IND][OF_ANGLE_IND] = dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass);
+    F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
+    F[OF_Z_IND][OF_Z_DOT_IND] = dt * 1.0f;
+    F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt * (-thrust * sinf(OF_X[OF_ANGLE_IND]) / mass);
+    if (OF_THRUST_BIAS) {
+      F[OF_V_IND][OF_THRUST_BIAS_IND] =  -dt * sinf(OF_X[OF_ANGLE_IND]) / mass;
+      F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt * cosf(OF_X[OF_ANGLE_IND]) / mass;
     }
   }
-  else {
-    F[OF_V_IND][OF_ANGLE_IND] = dt*(thrust*cosf(OF_X[OF_ANGLE_IND])/mass);
-    F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f;
-    F[OF_Z_IND][OF_Z_DOT_IND] = dt*1.0f;
-    F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt*(-thrust*sinf(OF_X[OF_ANGLE_IND])/mass);
-    if(OF_THRUST_BIAS) {
-	F[OF_V_IND][OF_THRUST_BIAS_IND] =  -dt*sinf(OF_X[OF_ANGLE_IND]) / mass;
-	F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt*cosf(OF_X[OF_ANGLE_IND]) / mass;
+
+  if (OF_DRAG) {
+    // In MATLAB: -sign(v)*2*kd*v/m (always minus, whether v is positive or negative):
+    F[OF_V_IND][OF_V_IND] -=  dt * 2 * kd * fabs(OF_X[OF_V_IND]) / mass;
+    if (OF_TWO_DIM) {
+      F[OF_VX_IND][OF_VX_IND] -=  dt * 2 * kd * fabs(OF_X[OF_VX_IND]) / mass;
     }
   }
 
@@ -975,46 +982,49 @@ void ins_flow_update(void)
   // Jacobian observation matrix H:
   float H[N_MEAS_OF_KF][N_STATES_OF_KF] = {{0.}};
 
-  if(CONSTANT_ALT_FILTER) {
-      // Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2];
+  if (CONSTANT_ALT_FILTER) {
+    // Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2];
     // lateral flow:
-    H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND];
-    H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
-    H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
+    H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
+    H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
+    H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) /
+                                   (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
     H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f;
     // divergence:
-    H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
-    H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
-    H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
+    H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
+    H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
+    H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
     H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f;
 
-    if(OF_TWO_DIM) {
-	// divergence measurement couples the two axes actually...:
-	H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2*OF_X[OF_THETA_IND])/(2*OF_X[OF_Z_IND]);
-	H[OF_DIV_FLOW_IND][OF_THETA_IND] = -OF_X[OF_VX_IND]*cosf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND];
-
-	// lateral flow in x direction:
-	H[OF_LAT_FLOW_X_IND][OF_VX_IND] = -cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/ OF_X[OF_Z_IND];
-	H[OF_LAT_FLOW_X_IND][OF_THETA_IND] = OF_X[OF_VX_IND]*sinf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND];
-	H[OF_LAT_FLOW_X_IND][OF_Z_IND] = OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
+    if (OF_TWO_DIM) {
+      // divergence measurement couples the two axes actually...:
+      H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2 * OF_X[OF_THETA_IND]) / (2 * OF_X[OF_Z_IND]);
+      H[OF_DIV_FLOW_IND][OF_THETA_IND] = -OF_X[OF_VX_IND] * cosf(2 * OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND];
+
+      // lateral flow in x direction:
+      H[OF_LAT_FLOW_X_IND][OF_VX_IND] = -cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND];
+      H[OF_LAT_FLOW_X_IND][OF_THETA_IND] = OF_X[OF_VX_IND] * sinf(2 * OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND];
+      H[OF_LAT_FLOW_X_IND][OF_Z_IND] = OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) /
+                                       (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
     }
   } else {
     // lateral flow:
-    H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND];
-    H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
-					  + OF_X[OF_Z_DOT_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
+    H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
+    H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+                                       + OF_X[OF_Z_DOT_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
     H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f;
-    H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND])
-				    - OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
-    H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
+    H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) /
+                                   (OF_X[OF_Z_IND] * OF_X[OF_Z_IND])
+                                   - OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
+    H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
     // divergence:
-    H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
-    H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
-					  + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
+    H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
+    H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+                                       + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
     H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f;
-    H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND])
-				    + OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
-    H[OF_DIV_FLOW_IND][OF_Z_DOT_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
+    H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND])
+                                   + OF_X[OF_Z_DOT_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
+    H[OF_DIV_FLOW_IND][OF_Z_DOT_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
   }
 
   // rate measurement:
@@ -1144,16 +1154,16 @@ void ins_flow_update(void)
     // Correct the state:
     // MATLAB:
     // Z_expected = [-v*cosf(theta)*cosf(theta)/z + zd*sinf(2*theta)/(2*z) + thetad;
-    //			(-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z];
+    //      (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z];
 
     float Z_expected[N_MEAS_OF_KF];
 
     // TODO: take this var out? It was meant for debugging...
     //float Z_expect_GT_angle;
 
-    if(CONSTANT_ALT_FILTER) {
-      Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
-				+OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze!
+    if (CONSTANT_ALT_FILTER) {
+      Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+                                    + OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze!
 
 
       /* TODO: remove later, just for debugging:
@@ -1166,41 +1176,41 @@ void ins_flow_update(void)
       }
       printf("\n");*/
 
-      Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
+      Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
 
-      if(OF_TWO_DIM) {
-	  Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; // TODO: no q?
+      if (OF_TWO_DIM) {
+        Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) /
+                                        OF_X[OF_Z_IND]; // TODO: no q?
       }
 
       //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND];
 
       if (OF_USE_GYROS) {
         Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
       }
-    }
-    else {
-      Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
-				     + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
-				     + OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter.
-							       // Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY!
+    } else {
+      Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+                                    + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
+                                    + OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter.
+      // Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY!
 
-      Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
-				    -OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
+      Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
+                                    - OF_X[OF_Z_DOT_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
 
       //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND]
-	//				     + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]);
-					     //+ OF_X[OF_ANGLE_DOT_IND];
-      if(N_MEAS_OF_KF == 3) {
-      	Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
+      //             + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]);
+      //+ OF_X[OF_ANGLE_DOT_IND];
+      if (N_MEAS_OF_KF == 3) {
+        Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
       }
 
-/*
-      float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
-			    + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
-      float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
-			      + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
-			      + OF_X[OF_ANGLE_DOT_IND];
-*/
+      /*
+            float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+                + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
+            float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+                  + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
+                  + OF_X[OF_ANGLE_DOT_IND];
+      */
       /*
       printf("Z_exp_no_rate = %f, Z_exp_with_rate = %f, measured = %f, angle dot = %f, p = %f: ", Z_exp_no_rate, Z_exp_with_rate,
        ins_flow.optical_flow_x, OF_X[OF_ANGLE_DOT_IND], dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f);
@@ -1472,8 +1482,9 @@ static void ins_act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t
 {
   ins_flow.RPM_num_act = num_act;
   for (int i = 0; i < num_act; i++) {
-    if(feedback[i].set.rpm)
+    if (feedback[i].set.rpm) {
       ins_flow.RPM[i] = feedback[i].rpm;
+    }
   }
 }
 

diff --git a/sw/tools/rtp_viewer/play_video_ffmpeg.sh b/sw/tools/rtp_viewer/play_video_ffmpeg.sh
@@ -0,0 +1 @@
+ffplay rtp_6000.sdp -protocol_whitelist file,udp,rtp -fflags nobuffer
diff --git a/sw/tools/rtp_viewer/rtp_viewer.py b/sw/tools/rtp_viewer/rtp_viewer.py
@@ -17,6 +17,9 @@
 from pprzlink.ivy import IvyMessagesInterface
 from pprzlink.message import PprzMessage
 
+# See the issue and solution here: https://github.com/opencv/opencv/issues/10328
+os.environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'protocol_whitelist;file,rtp,udp'
+
 class RtpViewer:
     running = False
     scale = 1
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		ffplay rtp_6000.sdp -protocol_whitelist file,udp,rtp -fflags nobuffer