[fix] Optical-Flow: configures not configuring, possible FAST9 segfau…

…lt, several parameters not active in OF (#3140)

conf/airframes/examples/bebop2_opticflow.xml

@@ -53,8 +53,6 @@
 
     <module name="cv_opticflow">
       <define name="MAX_HORIZON" value="10"/>
-      <configure name="OPTICFLOW_MAX_TRACK_CORNERS" value="20"/>
-      <configure name="OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2" value="20"/>
     </module>
 
     <module name="video_rtp_stream">
@@ -126,6 +124,7 @@
 
   <section name="OPTICAL_FLOW" prefix="OPTICFLOW_">
     <define name="CAMERA" value="bottom_camera"/>
+    <define name="MAX_TRACK_CORNERS" value="20"/>
 
     <define name="FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
     <define name="FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
@@ -141,6 +140,8 @@
 
 
     <define name="CAMERA2" value="front_camera"/>
+    <define name="MAX_TRACK_CORNERS_CAMERA2" value="20"/>
+
     <define name="DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="0.8"/>
     <define name="DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="0.85"/>
     <define name="FEATURE_MANAGEMENT_CAMERA2" value="0"/>
@@ -272,7 +273,7 @@
     <define name="REF_RATE_Q" value="28.0"/>
     <define name="REF_RATE_R" value="28.0"/>
 
-  	<define name="FILT_CUTOFF" value="3.2"/>
+    <define name="FILT_CUTOFF" value="3.2"/>
     <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->

conf/modules/cv_opticflow.xml

@@ -20,9 +20,9 @@
       <!-- Camera parameters -->
       <define name="DEROTATION_CORRECTION_FACTOR_X" value="1.0" description="Correction factor for derotation (in x direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
       <define name="DEROTATION_CORRECTION_FACTOR_Y" value="1.0" description="Correction factor for derotation (in y direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
-      <configure name="BODY_TO_CAM_PHI" value="0" description="Rotation from body frame to camera frame around x axis"/>
-      <configure name="BODY_TO_CAM_THETA" value="0" description="Rotation from body frame to camera frame around y axis"/>
-      <configure name="BODY_TO_CAM_PSI" value="0" description="Rotation from body frame to camera frame around z axis"/>
+      <define name="BODY_TO_CAM_PHI" value="0" description="Rotation from body frame to camera frame around x axis"/>
+      <define name="BODY_TO_CAM_THETA" value="0" description="Rotation from body frame to camera frame around y axis"/>
+      <define name="BODY_TO_CAM_PSI" value="0" description="Rotation from body frame to camera frame around z axis"/>
 
       <!-- General optical flow calculation parameters -->
       <define name="METHOD" value="0" description="Method used to calculate optical flow"/>
@@ -66,9 +66,9 @@
       <!-- Camera 2 parameters -->
       <define name="DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="1.0" description="Correction factor for derotation (in x direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
       <define name="DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="1.0" description="Correction factor for derotation (in y direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
-      <configure name="BODY_TO_CAM_PHI_CAMERA2" value="0" description="Rotation from body frame to camera frame around x axis"/>
-      <configure name="BODY_TO_CAM_THETA_CAMERA2" value="0" description="Rotation from body frame to camera frame around y axis"/>
-      <configure name="BODY_TO_CAM_PSI_CAMERA2" value="0" description="Rotation from body frame to camera frame around z axis"/>
+      <define name="BODY_TO_CAM_PHI_CAMERA2" value="0" description="Rotation from body frame to camera frame around x axis"/>
+      <define name="BODY_TO_CAM_THETA_CAMERA2" value="0" description="Rotation from body frame to camera frame around y axis"/>
+      <define name="BODY_TO_CAM_PSI_CAMERA2" value="0" description="Rotation from body frame to camera frame around z axis"/>
 
       <!-- General optical flow calculation parameters for camera 2-->
       <define name="METHOD_CAMERA2" value="0" description="Method used to calculate optical flow"/>

sw/airborne/modules/computer_vision/lib/vision/act_fast.c

@@ -27,17 +27,21 @@ All rights reserved.
 #include "math.h"
 #include "image.h"
 #include "../../opticflow/opticflow_calculator.h"
+#include "generated/airframe.h"
 
 // ACT-FAST agents arrays
 // equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
 // TODO Currently hardcoded to two cameras
 #define MAX_AGENTS FAST9_MAX_CORNERS
-#ifndef OPTICFLOW_CAMERA2
-  struct agent_t agents[1][MAX_AGENTS];
+#ifdef OPTICFLOW_CAMERA2
+#define FAST9_MAX_CAMERAS 2
 #else
-  struct agent_t agents[2][MAX_AGENTS];
+#define FAST9_MAX_CAMERAS 1
 #endif
 
+struct agent_t agents[FAST9_MAX_CAMERAS][MAX_AGENTS];
+
+
 /**
  * Do an ACT-FAST corner detection.
  * @param[in] *img The image to do the corner detection on
@@ -55,6 +59,11 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
               uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
               int gradient_method, int camera_id)
 {
+  // Input protection:
+  if (camera_id >= FAST9_MAX_CAMERAS) {
+    *num_corners = 0;
+    return;
+  }
 
   /*
    * Procedure:

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

@@ -45,6 +45,7 @@
 #include "size_divergence.h"
 #include "linear_flow_fit.h"
 #include "modules/sonar/agl_dist.h"
+#include "generated/airframe.h"
 
 // to get the definition of front_camera / bottom_camera
 #include BOARD_CONFIG

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -39,6 +39,7 @@
 #include "errno.h"
 
 #include "cv.h"
+#include "generated/airframe.h"
 
 uint16_t fps_OF;
 

sw/airborne/modules/sensors/baro_MS5534A.c

@@ -218,7 +218,7 @@ static void calibration(void)
 #ifndef BARO_NO_DOWNLINK
   float debug[4];
   for (int i=0; i<4; i++){
-    debug[0] = words[0];
+    debug[i] = words[i];
   }
   DOWNLINK_SEND_DEBUG_VECT(DefaultChannel, DefaultDevice, "baro_calib", 4, debug);
 #endif