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

@@ -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
 };
 

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<LogLevel> _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

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

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

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;

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)