diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 41f248c928..0e6e425098 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/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 }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 923d7bc0e9..a963a57fd7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index f84e4e767b..787152a779 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 6552f39b8c..b2cba6971f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/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) - EstimateTerrainOffset(); + 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(); + FuseOptFlow(); + } // reset flag to indicate that no new flow data is available for fusion flowDataToFuse = false; }