From 0d3e1a7d230a93b28df35786248408e87787c7a6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 15 Mar 2019 05:49:33 +1100 Subject: [PATCH] AP_NavEKF3: flow use parameter improvements --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 16 +++++++--------- libraries/AP_NavEKF3/AP_NavEKF3.h | 7 ++++++- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 6 +++--- 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index fcacb069dd..5d89b65306 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -34,7 +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 +#define FLOW_USE_DEFAULT 1 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2) // rover defaults @@ -59,7 +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 +#define FLOW_USE_DEFAULT 1 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) // plane defaults @@ -84,7 +84,7 @@ #define FLOW_M_NSE_DEFAULT 0.15f #define FLOW_I_GATE_DEFAULT 500 #define CHECK_SCALER_DEFAULT 100 -#define FLOW_USE_MASK_DEFAULT 2 +#define FLOW_USE_DEFAULT 2 #else // build type not specified, use copter defaults @@ -109,7 +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 +#define FLOW_USE_DEFAULT 1 #endif // APM_BUILD_DIRECTORY @@ -579,15 +579,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Units: m/s AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f), - // @Param: FLOW_MASK + // @Param: FLOW_USE // @DisplayName: Optical flow use bitmask - // @Description: Bitmask controlling if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator. + // @Description: Controls if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator. // @User: Advanced // @Values: 0:None,1:Navigation,2:Terrain - // @Bitmask: 0:Navigation,1:Terrain - // @Range: 0 2 // @RebootRequired: True - AP_GROUPINFO("FLOW_MASK", 54, NavEKF3, _flowUseMask, FLOW_USE_MASK_DEFAULT), + AP_GROUPINFO("FLOW_USE", 54, NavEKF3, _flowUse, FLOW_USE_DEFAULT), AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index a963a57fd7..d1e6bbd485 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -419,7 +419,12 @@ 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. + AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator. + +// Possible values for _flowUse +#define FLOW_USE_NONE 0 +#define FLOW_USE_NAV 1 +#define FLOW_USE_TERRAIN 2 // 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 787152a779..0b9800ad02 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() && (frontend->_flowUseMask & (1<<0))) || readyToUseBodyOdm()) { + } else if ((readyToUseOptFlow() && (frontend->_flowUse == FLOW_USE_NAV)) || 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 3a9308a5c7..501ffc28c6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -52,14 +52,14 @@ void NavEKF3_core::SelectFlowFusion() } // if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height - if (((flowDataToFuse && (frontend->_flowUseMask & (1<<1))) || rangeDataToFuse) && tiltOK) { + if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) { // Estimate the terrain offset (runs a one state EKF) EstimateTerrainOffset(); } // Fuse optical flow data into the main filter if (flowDataToFuse && tiltOK) { - if (frontend->_flowUseMask & (1<<0)) { + if (frontend->_flowUse == FLOW_USE_NAV) { // 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 @@ -89,7 +89,7 @@ void NavEKF3_core::EstimateTerrainOffset() // don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable // don't fuse flow data if it exceeds validity limits // don't update terrain offset if grpund is being used as the zero height datum in the main filter - bool cantFuseFlowData = (!(frontend->_flowUseMask & (1<<1)) + bool cantFuseFlowData = ((frontend->_flowUse != FLOW_USE_TERRAIN) || gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f