AP_NavEKF2: continuously update gpsGoodToAlign

use it to determine how to handle a height datum reset

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -540,13 +540,9 @@ void NavEKF2_core::readGpsData()
                 useGpsVertVel = false;
             }
 
-            // Monitor quality of the GPS velocity data before and after alignment using separate checks
-            if (PV_AidingMode != AID_ABSOLUTE) {
-                // Pre-alignment checks
-                gpsGoodToAlign = calcGpsGoodToAlign();
-            } else {
-                gpsGoodToAlign = false;
-            }
+            // Monitor quality of the GPS velocity data both before and after alignment. This updates
+            // GpsGoodToAlign class variable
+            calcGpsGoodToAlign();
 
             // Post-alignment checks
             calcGpsGoodForFlight();

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -595,7 +595,7 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan)
 // report the reason for why the backend is refusing to initialise
 const char *NavEKF2_core::prearm_failure_reason(void) const
 {
-    if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
+    if (gpsGoodToAlign) {
         // we are not failing
         return nullptr;
     }

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -214,21 +214,22 @@ bool NavEKF2_core::resetHeightDatum(void)
     // reset the height state
     stateStruct.position.z = 0.0f;
     // adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
-    if (validOrigin) {
-        const AP_GPS &gps = AP::gps();
-        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+
+    if (validOrigin && !inFlight) {
+        if (!gpsGoodToAlign) {
+            // if we don't have GPS lock then we shouldn't be doing a
+            // resetHeightDatum, but if we do then the best option is
+            // to maintain the old error
+            ekfGpsRefHgt += (int32_t)(100.0f * oldHgt);
+        } else {
             // if we have a good GPS lock then reset to the GPS
             // altitude. This ensures the reported AMSL alt from
             // getLLH() is equal to GPS altitude, while also ensuring
             // that the relative alt is zero
-            ekfGpsRefHgt = gps.location().alt*0.01;
-        } else {
-            // if we don't have GPS lock then we shouldn't be doing a
-            // resetHeightDatum, but if we do then the best option is
-            // to maintain the old error
-            ekfGpsRefHgt += oldHgt;
+            ekfGpsRefHgt = AP::gps().location().alt*0.01;
         }
     }
+
     // adjust the terrain state
     terrainState += oldHgt;
     return true;

libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

@@ -18,20 +18,33 @@ extern const AP_HAL::HAL& hal;
 
    We also record the failure reason so that prearm_failure_reason()
    can give a good report to the user on why arming is failing
+
+   This sets gpsGoodToAlign class variable
 */
-bool NavEKF2_core::calcGpsGoodToAlign(void)
+void NavEKF2_core::calcGpsGoodToAlign(void)
 {
     const AP_GPS &gps = AP::gps();
 
     if (inFlight && assume_zero_sideslip() && !use_compass()) {
         // this is a special case where a plane has launched without magnetometer
         // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
-        return true;
+        gpsGoodToAlign = true;
+        return;
     }
 
     // User defined multiplier to be applied to check thresholds
     float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
 
+    if (gpsGoodToAlign) {
+        /*
+          if we have already passed GPS alignment checks then raise
+          the check threshold so that we have some hysterisis and
+          don't continuously change from able to arm to not able to
+          arm
+         */
+        checkScaler *= 1.3f;
+    }
+
     // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
     // This enables us to handle large changes to the external magnetic field environment that occur before arming
     if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) {
@@ -223,16 +236,20 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
         lastGpsVelFail_ms = imuSampleTime_ms;
     }
 
-    // record time of fail
+    // record time of fail or pass
     if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || vAccFail ||  yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
         lastGpsVelFail_ms = imuSampleTime_ms;
+    } else {
+        lastGpsVelPass_ms = imuSampleTime_ms;
     }
 
-    // continuous period without fail required to return a healthy status
-    if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
-        return true;
+    // continuous period of 10s without fail required to set healthy
+    // continuous period of 5s without pass required to set unhealthy
+    if (!gpsGoodToAlign && imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
+        gpsGoodToAlign = true;
+    } else if (gpsGoodToAlign && imuSampleTime_ms - lastGpsVelPass_ms > 5000) {
+        gpsGoodToAlign = false;
     }
-    return false;
 }
 
 // update inflight calculaton that determines if GPS data is good enough for reliable navigation

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -134,6 +134,7 @@ void NavEKF2_core::InitialiseVariables()
     gndHgtValidTime_ms = 0;
     ekfStartTime_ms = imuSampleTime_ms;
     lastGpsVelFail_ms = 0;
+    lastGpsVelPass_ms = 0;
     lastGpsAidBadTime_ms = 0;
     timeTasReceived_ms = 0;
     lastPreAlignGpsCheckTime_ms = imuSampleTime_ms;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -718,8 +718,8 @@ private:
     // determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
     bool getTouchdownExpected();
 
-    // Assess GPS data quality and return true if good enough to align the EKF
-    bool calcGpsGoodToAlign(void);
+    // Assess GPS data quality and set gpsGoodToAlign if good enough to align the EKF
+    void calcGpsGoodToAlign(void);
 
     // return true and set the class variable true if the delta angle bias has been learned
     bool checkGyroCalStatus(void);
@@ -876,6 +876,7 @@ private:
     float gpsPosAccuracy;           // estimated position accuracy in m returned by the GPS receiver
     float gpsHgtAccuracy;           // estimated height accuracy in m returned by the GPS receiver
     uint32_t lastGpsVelFail_ms;     // time of last GPS vertical velocity consistency check fail
+    uint32_t lastGpsVelPass_ms;     // time of last GPS vertical velocity consistency check pass
     uint32_t lastGpsAidBadTime_ms;  // time in msec gps aiding was last detected to be bad
     float posDownAtTakeoff;         // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
     bool useGpsVertVel;             // true if GPS vertical velocity should be used