add OPTICAL_FLOW and VELOCITY_ESTIMATE ABI messages

and use them in the opticflow modules

conf/abi.xml

@@ -55,6 +55,23 @@
       <field name="gps_s" type="struct GpsState *"/>
     </message>
 
+    <message name="OPTICAL_FLOW" id="11">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="flow_x" type="int16_t">Flow in x direction from the camera (in subpixels)</field>
+      <field name="flow_y" type="int16_t">Flow in y direction from the camera (in subpixels)</field>
+      <field name="flow_der_x" type="int16_t">The derotated flow calculation in the x direction (in subpixels)</field>
+      <field name="flow_der_y" type="int16_t">The derotated flow calculation in the y direction (in subpixels)</field>
+      <field name="quality" type="uint8_t"/>
+      <field name="dist" type="float">Distance to ground</field>
+    </message>
+
+    <message name="VELOCITY_ESTIMATE" id="12">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="x" type="float" unit="m/s"/>
+      <field name="y" type="float" unit="m/s"/>
+      <field name="z" type="float" unit="m/s"/>
+    </message>
+
   </msg_class>
 
 </protocol>

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c

@@ -179,6 +179,10 @@ void guidance_h_init(void)
 
   gh_ref_init();
 
+#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
+  guidance_h_module_init();
+#endif
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_INT", send_gh);
   register_periodic_telemetry(DefaultPeriodic, "HOVER_LOOP", send_hover_loop);

sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h

@@ -33,6 +33,7 @@
  *
  * a) Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE
  * One must then implement:
+ * - void guidance_h_module_init(void);
  * - void guidance_h_module_enter(void);
  * - void guidance_h_module_read_rc(void);
  * - void guidance_h_module_run(bool_t in_flight);

sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c

@@ -30,12 +30,20 @@
 // Own Header
 #include "stabilization_opticflow.h"
 
+#include "subsystems/abi.h"
+
 // Stabilization
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/guidance/guidance_v.h"
 #include "autopilot.h"
 #include "subsystems/datalink/downlink.h"
 
+/** Default sender to accect VELOCITY_ESTIMATE messages from */
+#ifndef VISION_VELOCITY_ESTIMATE_ID
+#define VISION_VELOCITY_ESTIMATE_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(VISION_VELOCITY_ESTIMATE_ID)
+
 #define CMD_OF_SAT  1500 // 40 deg = 2859.1851
 
 #ifndef VISION_PHI_PGAIN
@@ -76,6 +84,8 @@ PRINT_CONFIG_VAR(VISION_DESIRED_VY)
 #error "ALL control gains have to be positive!!!"
 #endif
 
+static abi_event velocity_est_ev;
+
 /* Initialize the default gains and settings */
 struct opticflow_stab_t opticflow_stab = {
   .phi_pgain = VISION_PHI_PGAIN,
@@ -86,6 +96,18 @@ struct opticflow_stab_t opticflow_stab = {
   .desired_vy = VISION_DESIRED_VY
 };
 
+
+static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
+                                           uint32_t stamp, float vel_x, float vel_y, float vel_z);
+/**
+ * Initialization of horizontal guidance module.
+ */
+void guidance_h_module_init(void)
+{
+  // Subscribe to the VELOCITY_ESTIMATE ABI message
+  AbiBindMsgVELOCITY_ESTIMATE(VISION_VELOCITY_ESTIMATE_ID, &velocity_est_ev, stabilization_opticflow_vel_cb);
+}
+
 /**
  * Horizontal guidance mode enter resets the errors
  * and starts the controller.
@@ -124,23 +146,19 @@ void guidance_h_module_run(bool_t in_flight)
 }
 
 /**
- * Update the controls based on a vision result
+ * Update the controls on a new VELOCITY_ESTIMATE ABI message.
- * @param[in] *result The opticflow calculation result used for control
  */
-void stabilization_opticflow_update(struct opticflow_result_t *result)
+static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
+                                           uint32_t stamp, float vel_x, float vel_y, float vel_z)
 {
   /* Check if we are in the correct AP_MODE before setting commands */
   if (autopilot_mode != AP_MODE_MODULE) {
     return;
   }
 
-  /* Calculate the error if we have enough flow */
+  /* Calculate the error */
-  float err_vx = 0;
+  float err_vx = opticflow_stab.desired_vx - vel_x;
-  float err_vy = 0;
+  float err_vy = opticflow_stab.desired_vy - vel_y;
-  if (result->tracked_cnt > 0) {
-    err_vx = opticflow_stab.desired_vx - result->vel_x;
-    err_vy = opticflow_stab.desired_vy - result->vel_y;
-  }
 
   /* Calculate the integrated errors (TODO: bound??) */
   opticflow_stab.err_vx_int += err_vx / 100;

sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.h

@@ -31,8 +31,6 @@
 #define CV_STABILIZATION_OPTICFLOW_H_
 
 #include "std.h"
-#include "lib/v4l/v4l2.h"
-#include "inter_thread_data.h"
 #include "math/pprz_algebra_int.h"
 
 /* The opticflow stabilization */
@@ -51,11 +49,9 @@ struct opticflow_stab_t {
 extern struct opticflow_stab_t opticflow_stab;
 
 // Implement own Horizontal loops
+extern void guidance_h_module_init(void);
 extern void guidance_h_module_enter(void);
 extern void guidance_h_module_read_rc(void);
 extern void guidance_h_module_run(bool_t in_flight);
 
-// Update the stabiliztion commands based on a vision result
-void stabilization_opticflow_update(struct opticflow_result_t *vision);
-
 #endif /* CV_STABILIZATION_OPTICFLOW_H_ */

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -43,6 +43,10 @@
 #endif
 PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
 
+#ifndef OPTICFLOW_SENDER_ID
+#define OPTICFLOW_SENDER_ID 1
+#endif
+
 /* The video device */
 #ifndef OPTICFLOW_DEVICE
 #define OPTICFLOW_DEVICE /dev/video2      ///< The video device
@@ -151,7 +155,21 @@ void opticflow_module_run(void)
 
   // Update the stabilization loops on the current calculation
   if (opticflow_got_result) {
-    stabilization_opticflow_update(&opticflow_result);
+    uint32_t now_ts = get_sys_time_usec();
+    uint8_t quality = opticflow_result.divergence; // FIXME, scale to some quality measure 0-255
+    AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts,
+                           opticflow_result.flow_x,
+                           opticflow_result.flow_y,
+                           opticflow_result.flow_der_x,
+                           opticflow_result.flow_der_x,
+                           quality,
+                           opticflow_state.agl);
+    if (opticflow_result.tracked_cnt > 0) {
+      AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
+                                  opticflow_result.vel_x,
+                                  opticflow_result.vel_y,
+                                  0.0f);
+    }
     opticflow_got_result = FALSE;
   }
   pthread_mutex_unlock(&opticflow_mutex);

sw/airborne/modules/ctrl/ctrl_module_demo.c

@@ -77,6 +77,11 @@ void ctrl_module_run(bool_t in_flight)
 ////////////////////////////////////////////////////////////////////
 // Call our controller
 // Implement own Horizontal loops
+void guidance_h_module_init(void)
+{
+  ctrl_module_init();
+}
+
 void guidance_h_module_enter(void)
 {
   ctrl_module_init();

sw/airborne/modules/ctrl/ctrl_module_demo.h

@@ -44,6 +44,7 @@ extern float ctrl_module_demo_y_d_gain;
 #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
 
 // Implement own Horizontal loops
+extern void guidance_h_module_init(void);
 extern void guidance_h_module_enter(void);
 extern void guidance_h_module_read_rc(void);
 extern void guidance_h_module_run(bool_t in_flight);