[INS] added extra noise_measurement value to abi message VELOCITY_ESTIMATE, which is calculated in optic flow module

conf/abi.xml

@@ -70,6 +70,7 @@
       <field name="x" type="float" unit="m/s"/>
       <field name="y" type="float" unit="m/s"/>
       <field name="z" type="float" unit="m/s"/>
+      <field name="noise" type="float"/>
     </message>
 
   </msg_class>

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

@@ -47,6 +47,8 @@ struct opticflow_result_t {
 
   float surface_roughness; ///< Surface roughness as determined with a linear optical flow fit
   float divergence;       ///< Divergence as determined with a linear flow fit
+
+  float noise_measurement;  ///< noise of measurement, for state filter
 };
 
 /* The state of the drone when it took an image */

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

@@ -152,7 +152,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
  * @param[in] *img The image frame to calculate the optical flow from
  * @param[out] *result The optical flow result
  */
-void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result)
+void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
+                          struct opticflow_result_t *result)
 {
   // variables for size_divergence:
   float size_divergence; int n_samples;
@@ -230,7 +231,8 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
     error_threshold = 10.0f;
     n_iterations_RANSAC = 20;
     n_samples_RANSAC = 5;
-    success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC, n_samples_RANSAC, img->w, img->h, &fit_info);
+    success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC,
+                                            n_samples_RANSAC, img->w, img->h, &fit_info);
 
     if (!success_fit) {
       fit_info.divergence = 0.0f;
@@ -279,6 +281,14 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   result->vel_x = -result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * img->w / OPTICFLOW_FX;
   result->vel_y =  result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * img->h / OPTICFLOW_FY;
 
+  // Determine quality of noise measurement for state filter
+  //TODO Experiment with multiple noise measurement models
+  if (result->tracked_cnt < 15) {
+    result->noise_measurement = (float)result->tracked_cnt / (float)opticflow->max_track_corners;
+  } else {
+    result->noise_measurement = 1.0;
+  }
+
   // *************************************************************************************
   // Next Loop Preparation
   // *************************************************************************************
@@ -312,7 +322,8 @@ static int cmp_flow(const void *a, const void *b)
 {
   const struct flow_t *a_p = (const struct flow_t *)a;
   const struct flow_t *b_p = (const struct flow_t *)b;
-  return (a_p->flow_x * a_p->flow_x + a_p->flow_y * a_p->flow_y) - (b_p->flow_x * b_p->flow_x + b_p->flow_y * b_p->flow_y);
+  return (a_p->flow_x * a_p->flow_x + a_p->flow_y * a_p->flow_y) - (b_p->flow_x * b_p->flow_x + b_p->flow_y *
+         b_p->flow_y);
 }
 
 

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -131,7 +131,8 @@ void opticflow_module_init(void)
 #endif
 
   /* Try to initialize the video device */
-  opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS, V4L2_PIX_FMT_UYVY);
+  opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS,
+                            V4L2_PIX_FMT_UYVY);
   if (opticflow_dev == NULL) {
     printf("[opticflow_module] Could not initialize the video device\n");
   }
@@ -168,7 +169,9 @@ void opticflow_module_run(void)
       AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
                                   opticflow_result.vel_x,
                                   opticflow_result.vel_y,
-                                  opticflow_result.tracked_cnt);
+                                  0.0f,
+                                  opticflow_result.noise_measurement
+                                 );
     }
     opticflow_got_result = FALSE;
   }

sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c

@@ -98,7 +98,7 @@ struct opticflow_stab_t opticflow_stab = {
 
 
 static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
-                                           uint32_t stamp, float vel_x, float vel_y, float vel_z);
+    uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise);
 /**
  * Initialization of horizontal guidance module.
  */
@@ -149,7 +149,7 @@ void guidance_h_module_run(bool_t in_flight)
  * Update the controls on a new VELOCITY_ESTIMATE ABI message.
  */
 static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
-                                           uint32_t stamp, float vel_x, float vel_y, float vel_z)
+    uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise)
 {
   /* Check if we are in the correct AP_MODE before setting commands */
   if (autopilot_mode != AP_MODE_MODULE) {

sw/airborne/subsystems/ins/ins_int.c

@@ -141,7 +141,7 @@ static abi_event gps_ev;
 #define INS_INT_VEL_ID ABI_BROADCAST
 #endif
 static abi_event vel_est_ev;
-static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z);
+static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise);
 
 /** measurement noise for velocity estimate */
 #ifndef INS_VFF_R_VEL
@@ -238,8 +238,7 @@ void ins_reset_local_origin(void)
     ins_int.ltp_def.hmsl = gps.hmsl;
     ins_int.ltp_initialized = TRUE;
     stateSetLocalOrigin_i(&ins_int.ltp_def);
-  }
-  else {
+  } else {
     ins_int.ltp_initialized = FALSE;
   }
 #else
@@ -306,7 +305,7 @@ void ins_int_propagate(struct Int32Vect3 *accel, float dt)
   ins_ned_to_state();
 
   /* increment the propagation counter, while making sure it doesn't overflow */
-  if (ins_int.propagation_cnt < 100*INS_MAX_PROPAGATION_STEPS) {
+  if (ins_int.propagation_cnt < 100 * INS_MAX_PROPAGATION_STEPS) {
     ins_int.propagation_cnt++;
   }
 }
@@ -523,17 +522,17 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 
 static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
                        uint32_t stamp __attribute__((unused)),
-                       float x, float y, float z)
+                       float x, float y, float z, float noise)
 {
 
-  struct FloatVect3 vel_body = {x, y, 0};
+  struct FloatVect3 vel_body = {x, y, z};
 
   /* rotate velocity estimate to nav/ltp frame */
 
   // from frame coordinates to body coordinates and convert cm/s-> m/s
-  //TODO Do this in the optical flow module already
-  vel_body.x=y/100;
-  vel_body.y=x/100;
+  //TODO Do this in the optical flow module already, but after the guidance opticflow module has disapeared
+  vel_body.x = y / 100;
+  vel_body.y = x / 100;
 
   struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
   QUAT_INVERT(q_b2n, q_b2n);
@@ -542,12 +541,7 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
 
 #if USE_HFF
   struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
-  //struct FloatVect2 Rvel = {INS_VFF_R_VEL, INS_VFF_R_VEL};
-  //FIXME this should come out of optical flow module
-  if(z<17)
-      struct FloatVect2 Rvel = {z/25,z/25};
-  else
-	  struct FloatVect2 Rvel = {1.0,1.0};
+  struct FloatVect2 Rvel = {noise, noise};
 
   b2_hff_update_vel(vel,  Rvel);
   ins_update_from_hff();