Added camera id to ABI message

Author: guidoAI

conf/abi.xml

@@ -65,6 +65,7 @@
       <field name="flow_der_y" type="int32_t">The derotated flow calculation in the y direction (in subpixels)</field>
       <field name="quality" type="float"/>
       <field name="size_divergence" type="float">Divergence as determined with the size method (in 1/seconds) with LK, and Divergence (1/seconds) itself with EF</field>
+      <field name="camera_id" type="uint8_t">Camera ID (bottom or front)</field>
     </message>
 
     <message name="VELOCITY_ESTIMATE" id="12">

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -150,7 +150,8 @@ void opticflow_module_run(void)
                              opticflow_result[idx_camera].flow_der_x,
                              opticflow_result[idx_camera].flow_der_y,
                              opticflow_result[idx_camera].noise_measurement,
-                             opticflow_result[idx_camera].div_size);
+                             opticflow_result[idx_camera].div_size,
+			     (uint8_t) idx_camera);
       //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
       if (opticflow_result[idx_camera].noise_measurement < 0.8) {
         AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts,

sw/airborne/modules/ctrl/optical_flow_landing.c

@@ -271,7 +271,7 @@ static void send_divergence(struct transport_tx *trans, struct link_device *dev)
 void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
 // Callback function of the optical flow estimate:
 void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
-                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
+                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id);
 
 // common functions for different landing strategies:
 static void set_cov_div(int32_t thrust);
@@ -1107,14 +1107,19 @@ void vertical_ctrl_agl_cb(uint8_t sender_id UNUSED, __attribute__((unused)) uint
 
 void vertical_ctrl_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp,
                                    int32_t flow_x UNUSED, int32_t flow_y UNUSED,
-                                   int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, float quality UNUSED, float size_divergence)
+                                   int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED,
+				   float quality UNUSED, float size_divergence,
+				   uint8_t camera_id)
 {
 
-  optical_flow_x = ((float)flow_der_x)/10.0;
-  optical_flow_y = ((float)flow_der_y)/10.0;
-  divergence_vision = size_divergence;
-  //printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision);
-  vision_time = ((float)stamp) / 1e6;
+  if(camera_id == 0)
+  {
+    optical_flow_x = ((float)flow_der_x)/10.0;
+    optical_flow_y = ((float)flow_der_y)/10.0;
+    divergence_vision = size_divergence;
+    //printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision);
+    vision_time = ((float)stamp) / 1e6;
+  }
 }
 
 ////////////////////////////////////////////////////////////////////

sw/airborne/modules/loggers/file_logger.c

@@ -91,12 +91,13 @@ int16_t OF_flow_der_x;
 int16_t OF_flow_der_y;
 float OF_quality;
 float OF_size_divergence;
+uint8_t OF_camera_id;
 static abi_event OF_ev; ///< The sonar ABI event
 static void logger_optical_flow_cb(uint8_t sender_id __attribute__((unused)),uint32_t stamp, int32_t flow_x,
-                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
+                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id);
 
 static void logger_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
-                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
+                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id)
 {
   OF_previous_stamp = OF_stamp;
   OF_stamp = stamp;
@@ -106,6 +107,7 @@ static void logger_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t fl
   OF_flow_der_y = flow_der_y;
   OF_quality = quality;
   OF_size_divergence = size_divergence;
+  OF_camera_id = camera_id;
 }
 uint32_t OF_stamp2=0;
 uint32_t OF_previous_stamp2=0;
@@ -115,12 +117,13 @@ int16_t OF_flow_der_x2;
 int16_t OF_flow_der_y2;
 float OF_quality2;
 float OF_size_divergence2;
+uint8_t OF_camera_id2;
 static abi_event OF_ev2; ///< The sonar ABI event
 static void logger_optical_flow_cb2(uint8_t sender_id __attribute__((unused)),uint32_t stamp, int32_t flow_x,
-                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
+                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id);
 
 static void logger_optical_flow_cb2(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
-                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
+                                   int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id)
 {
   OF_previous_stamp2 = OF_stamp;
   OF_stamp2 = stamp;
@@ -130,6 +133,7 @@ static void logger_optical_flow_cb2(uint8_t sender_id, uint32_t stamp, int32_t f
   OF_flow_der_y2 = flow_der_y;
   OF_quality2 = quality;
   OF_size_divergence2 = size_divergence;
+  OF_camera_id2 = camera_id;
 }
 
 // reading RPMs: