AP_NavEKF3: Add parameter to control how optical flow data is used

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -34,6 +34,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
+#define FLOW_USE_MASK_DEFAULT   1
 
 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
 // rover defaults
@@ -58,6 +59,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
+#define FLOW_USE_MASK_DEFAULT   1
 
 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
 // plane defaults
@@ -82,6 +84,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
+#define FLOW_USE_MASK_DEFAULT   2
 
 #else
 // build type not specified, use copter defaults
@@ -106,6 +109,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
+#define FLOW_USE_MASK_DEFAULT   1
 
 #endif // APM_BUILD_DIRECTORY
 
@@ -575,6 +579,15 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
     // @Units: m/s
     AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f),
 
+    // @Param: FLOW_MASK
+    // @DisplayName: Optical flow use bitmask
+    // @Description: Bitmask controlling if the optical flow data is fused into the 24-state navigation estimator and/or the 1-state terrain height estimator.
+    // @User: Advanced
+    // @Values: 0:None,1:Navgation,2:Terrain,3:Both
+    // @Bitmask: 0:Navigation,1:Terrain
+    // @RebootRequired: True
+    AP_GROUPINFO("FLOW_MASK", 54, NavEKF3, _flowUseMask, FLOW_USE_MASK_DEFAULT),
+
     AP_GROUPEND
 };
 

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -419,7 +419,7 @@ private:
     AP_Float _visOdmVelErrMax;      // Observation 1-STD velocity error assumed for visual odometry sensor at lowest reported quality (m/s)
     AP_Float _visOdmVelErrMin;      // Observation 1-STD velocity error assumed for visual odometry sensor at highest reported quality (m/s)
     AP_Float _wencOdmVelErr;        // Observation 1-STD velocity error assumed for wheel odometry sensor (m/s)
-
+    AP_Int8  _flowUseMask;          // Bitmask controlling if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
 
     // Tuning parameters
     const float gpsNEVelVarAccScale = 0.05f;       // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -209,7 +209,7 @@ void NavEKF3_core::setAidingMode()
             // GPS aiding is the preferred option unless excluded by the user
             if(readyToUseGPS() || readyToUseRangeBeacon()) {
                 PV_AidingMode = AID_ABSOLUTE;
-            } else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
+            } else if ((readyToUseOptFlow()  && (frontend->_flowUseMask & (1<<0))) || readyToUseBodyOdm()) {
                 PV_AidingMode = AID_RELATIVE;
             }
             break;

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -57,16 +57,20 @@ void NavEKF3_core::SelectFlowFusion()
         // fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
         fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse);
         // Estimate the terrain offset (runs a one state EKF)
+        if (frontend->_flowUseMask & (1<<1)) {
             EstimateTerrainOffset();
         }
+    }
 
     // Fuse optical flow data into the main filter
     if (flowDataToFuse && tiltOK)
     {
+        if (frontend->_flowUseMask & (1<<0)) {
         // Set the flow noise used by the fusion processes
         R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
         // Fuse the optical flow X and Y axis data into the main filter sequentially
             FuseOptFlow();
+        }
         // reset flag to indicate that no new flow data is available for fusion
         flowDataToFuse = false;
     }