From 8038efc15234eeaba165d5bae36e3002287f8681 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 15 Mar 2019 05:45:28 +1100 Subject: [PATCH] AP_NavEKF2: Flow use parameter updates --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 16 +++++++--------- libraries/AP_NavEKF2/AP_NavEKF2.h | 7 ++++++- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- .../AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 6 +++--- 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 31063b4932..4cced2a1d5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -35,7 +35,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 @@ -61,7 +61,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 @@ -87,7 +87,7 @@ #define FLOW_M_NSE_DEFAULT 0.15f #define FLOW_I_GATE_DEFAULT 500 #define CHECK_SCALER_DEFAULT 150 -#define FLOW_USE_MASK_DEFAULT 2 +#define FLOW_USE_DEFAULT 2 #else // build type not specified, use copter defaults @@ -113,7 +113,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 @@ -557,15 +557,13 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @RebootRequired: True AP_GROUPINFO("EXTNAV_DELAY", 50, NavEKF2, _extnavDelay_ms, 10), - // @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", 51, NavEKF2, _flowUseMask, FLOW_USE_MASK_DEFAULT), + AP_GROUPINFO("FLOW_USE", 51, NavEKF2, _flowUse, FLOW_USE_DEFAULT), AP_GROUPEND }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 8017426d3e..882355a5ac 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -399,7 +399,12 @@ 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. + 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_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 668b923823..41e4a79382 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/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() && (frontend->_flowUseMask & (1<<0)) && filterIsStable) { + } else if (optFlowDataPresent() && (frontend->_flowUse == FLOW_USE_NAV) && filterIsStable) { PV_AidingMode = AID_RELATIVE; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 1e6cd4fb6d..aa13ad939e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -49,14 +49,14 @@ void NavEKF2_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 @@ -86,7 +86,7 @@ void NavEKF2_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