From 8959d35b9116bb91d79948e9295e12497372127a Mon Sep 17 00:00:00 2001 From: Tamas Nepusz Date: Fri, 29 Jul 2022 19:49:37 +0200 Subject: [PATCH] AP_NavEKF3: add EK3_GPS_VACC_MAX as a threshold that decides whether to use GPS as altitude source depending on vAcc --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 9 ++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 1 + .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 46 +++++++++++++++---- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 7 ++- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 8 +++- 6 files changed, 59 insertions(+), 14 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index be9ffa77d2..0fa49a6237 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -728,6 +728,15 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @User: Advanced AP_GROUPINFO("LOG_LEVEL", 9, NavEKF3, _log_level, 0), + // @Param: GPS_VACC_MAX + // @DisplayName: GPS vertical accuracy threshold + // @Description: Vertical accuracy threshold for GPS as the altitude source. The GPS will not be used as an altitude source if the reported vertical accuracy of the GPS is larger than this threshold, falling back to baro instead. Set to zero to deactivate the threshold check. + // @Range: 0.0 10.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: m + AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 4b0e4d1a40..8979b1c299 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -433,6 +433,7 @@ private: AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m) AP_Int8 _primary_core; // initial core number AP_Enum _log_level; // log verbosity level + AP_Float _gpsVAccThreshold; // vertical accuracy threshold to use GPS as an altitude source // Possible values for _flowUse #define FLOW_USE_NONE 0 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 8501e7fd9f..41a78c6a35 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1135,7 +1135,7 @@ void NavEKF3_core::selectHeightForFusion() // Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh); - bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); + bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000 || !gpsAccuracyGoodForAltitude)); bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000)); bool fallback_to_baro = lostRngHgt || lostGpsHgt || lostRngBcnHgt; #if EK3_FEATURE_EXTERNAL_NAV diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index f00f558f56..e2386001db 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -237,9 +237,11 @@ void NavEKF3_core::calcGpsGoodToAlign(void) // update inflight calculaton that determines if GPS data is good enough for reliable navigation void NavEKF3_core::calcGpsGoodForFlight(void) { - // use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks + // use simple criteria based on the GPS receiver's claimed vertical + // position accuracy and speed accuracy and the EKF innovation consistency + // checks - // set up varaibles and constants used by filter that is applied to GPS speed accuracy + // set up variables and constants used by filter that is applied to GPS speed accuracy const ftype alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data const ftype tau = 10.0f; // time constant (sec) of peak hold decay if (lastGpsCheckTime_ms == 0) { @@ -268,25 +270,51 @@ void NavEKF3_core::calcGpsGoodForFlight(void) gpsSpdAccPass = true; } + // Apply a threshold test with hysteresis to the GPS vertical position accuracy data + // Require a fail for one second and a pass for 3 seconds to transition + float gpsVAccRaw; + ftype gpsVAccThreshold = (ftype)frontend->_gpsVAccThreshold; + if (lastGpsVertAccFailTime_ms == 0) { + lastGpsVertAccFailTime_ms = imuSampleTime_ms; + lastGpsVertAccPassTime_ms = imuSampleTime_ms; + } + if (!dal.gps().vertical_accuracy(preferred_gps, gpsVAccRaw)) { + // No vertical accuracy data yet, let's treat it as a value above the threshold + gpsVAccRaw = gpsVAccThreshold + 1.0f; + } + if (gpsVAccThreshold <= 0 || gpsVAccRaw < gpsVAccThreshold) { + lastGpsVertAccPassTime_ms = imuSampleTime_ms; + } else { + lastGpsVertAccFailTime_ms = imuSampleTime_ms; + } + if ((imuSampleTime_ms - lastGpsVertAccPassTime_ms) > 1000) { + gpsVertAccPass = false; + } else if ((imuSampleTime_ms - lastGpsVertAccFailTime_ms) > 3000) { + gpsVertAccPass = true; + } + // Apply a threshold test with hysteresis to the normalised position and velocity innovations // Require a fail for one second and a pass for 10 seconds to transition - if (lastInnovFailTime_ms == 0) { - lastInnovFailTime_ms = imuSampleTime_ms; - lastInnovPassTime_ms = imuSampleTime_ms; + if (lastGpsInnovFailTime_ms == 0) { + lastGpsInnovFailTime_ms = imuSampleTime_ms; + lastGpsInnovPassTime_ms = imuSampleTime_ms; } if (velTestRatio < 1.0f && posTestRatio < 1.0f) { - lastInnovPassTime_ms = imuSampleTime_ms; + lastGpsInnovPassTime_ms = imuSampleTime_ms; } else if (velTestRatio > 0.7f || posTestRatio > 0.7f) { - lastInnovFailTime_ms = imuSampleTime_ms; + lastGpsInnovFailTime_ms = imuSampleTime_ms; } - if ((imuSampleTime_ms - lastInnovPassTime_ms) > 1000) { + if ((imuSampleTime_ms - lastGpsInnovPassTime_ms) > 1000) { ekfInnovationsPass = false; - } else if ((imuSampleTime_ms - lastInnovFailTime_ms) > 10000) { + } else if ((imuSampleTime_ms - lastGpsInnovFailTime_ms) > 10000) { ekfInnovationsPass = true; } // both GPS speed accuracy and EKF innovations must pass gpsAccuracyGood = gpsSpdAccPass && ekfInnovationsPass; + + // also update whether the GPS fix is good enough for altitude + gpsAccuracyGoodForAltitude = gpsAccuracyGood && gpsVertAccPass; } // Detect if we are in flight or on ground diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index ac86dde5b1..797d317935 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -294,9 +294,12 @@ void NavEKF3_core::InitialiseVariables() sAccFilterState1 = 0.0f; sAccFilterState2 = 0.0f; lastGpsCheckTime_ms = 0; - lastInnovPassTime_ms = 0; - lastInnovFailTime_ms = 0; + lastGpsInnovPassTime_ms = 0; + lastGpsInnovFailTime_ms = 0; + lastGpsVertAccPassTime_ms = 0; + lastGpsVertAccFailTime_ms = 0; gpsAccuracyGood = false; + gpsAccuracyGoodForAltitude = false; gpsloc_prev = {}; gpsDriftNE = 0.0f; gpsVertVelFilt = 0.0f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 01f7e81b7a..d80415e126 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1163,13 +1163,17 @@ private: // variable used by the in-flight GPS quality check bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks + bool gpsVertAccPass; // true when reported GPS vertical accuracy passes in-flight checks bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks ftype sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy ftype sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked - uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed - uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed + uint32_t lastGpsInnovPassTime_ms; // last time in msec the GPS innovations passed + uint32_t lastGpsInnovFailTime_ms; // last time in msec the GPS innovations failed + uint32_t lastGpsVertAccPassTime_ms; // last time in msec the GPS vertical accuracy test passed + uint32_t lastGpsVertAccFailTime_ms; // last time in msec the GPS vertical accuracy test failed bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight. + bool gpsAccuracyGoodForAltitude; // true when the GPS accuracy is considered to be good enough to use it as an altitude source. Vector3F gpsVelInnov; // gps velocity innovations Vector3F gpsVelVarInnov; // gps velocity innovation variances uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts)