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

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -35,6 +35,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
+#define FLOW_USE_MASK           1
 
 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
 // rover defaults
@@ -60,6 +61,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
+#define FLOW_USE_MASK           1
 
 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
 // plane defaults
@@ -85,6 +87,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    150
+#define FLOW_USE_MASK           2
 
 #else
 // build type not specified, use copter defaults
@@ -110,6 +113,7 @@
 #define FLOW_M_NSE_DEFAULT      0.25f
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
+#define FLOW_USE_MASK           3
 
 #endif // APM_BUILD_DIRECTORY
 
@@ -553,6 +557,15 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @RebootRequired: True
     AP_GROUPINFO("EXTNAV_DELAY", 50, NavEKF2, _extnavDelay_ms, 10),
 
+    // @Param: FLOW_MASK
+    // @DisplayName: Optical flow use bitmask
+    // @Description: Bitmask controlling if the optical flow dara is fused into the main navigation estimator and/or the terrain estimator.
+    // @User: Advanced
+    // @Values: 0:None,1:Navigation,2:Terrain,3:Both
+    // @Bitmask: 0:Navigation,1:Terrain
+    // @RebootRequired: True
+    AP_GROUPINFO("FLOW_MASK", 51, NavEKF2, _flowUseMask, FLOW_USE_MASK),
+
     AP_GROUPEND
 };
 

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -399,6 +399,7 @@ private:
     AP_Int8 _magMask;               // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion.
     AP_Int8 _originHgtMode;         // Bitmask controlling post alignment correction and reporting of the EKF origin height.
     AP_Int8 _extnavDelay_ms;        // effective average delay of external nav system measurements relative to inertial measurements (msec)
+    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_NavEKF2/AP_NavEKF2_Control.cpp

@@ -170,7 +170,7 @@ void NavEKF2_core::setAidingMode()
         bool canUseExtNav = readyToUseExtNav();
         if(canUseGPS || canUseRangeBeacon || canUseExtNav) {
             PV_AidingMode = AID_ABSOLUTE;
-        } else if (optFlowDataPresent() && filterIsStable) {
+        } else if (optFlowDataPresent() && (frontend->_flowUseMask & (1<<0)) && filterIsStable) {
             PV_AidingMode = AID_RELATIVE;
         }
         }

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -54,16 +54,19 @@ void NavEKF2_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)
-        EstimateTerrainOffset();
+        if (frontend->_flowUseMask & (1<<1)) {
+            EstimateTerrainOffset();
+        }
     }
 
     // Fuse optical flow data into the main filter
-    if (flowDataToFuse && tiltOK)
-    {
+    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();
+            FuseOptFlow();
+        }
         // reset flag to indicate that no new flow data is available for fusion
         flowDataToFuse = false;
     }