AP_NavEKF3: continuously update gpsGoodToAlign

use it to determine how to handle a height datum reset

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -576,13 +576,8 @@ void NavEKF3_core::readGpsData()
                 useGpsVertVel = false;
             }
 
-            // Monitor quality of the GPS velocity data before and after alignment using separate checks
+            // Monitor quality of the GPS velocity data before and after alignment
-            if (PV_AidingMode != AID_ABSOLUTE) {
+            calcGpsGoodToAlign();
-                // Pre-alignment checks
-                gpsGoodToAlign = calcGpsGoodToAlign();
-            } else {
-                gpsGoodToAlign = false;
-            }
 
             // Post-alignment checks
             calcGpsGoodForFlight();

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

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

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -212,21 +212,21 @@ bool NavEKF3_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) {
+    if (validOrigin && !inFlight) {
-        const AP_GPS &gps = AP::gps();
+        if (!gpsGoodToAlign) {
-        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+            // 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;
+            ekfGpsRefHgt = AP::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;
         }
     }
+
     // adjust the terrain state
     terrainState += oldHgt;
     return true;

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -17,17 +17,28 @@ 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
 */
-bool NavEKF3_core::calcGpsGoodToAlign(void)
+void NavEKF3_core::calcGpsGoodToAlign(void)
 {
     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) {
@@ -222,16 +233,20 @@ bool NavEKF3_core::calcGpsGoodToAlign(void)
         lastGpsVelFail_ms = imuSampleTime_ms;
     }
 
-    // record time of fail
+    // record time of pass or fail
     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
+    // continuous period of 10s without fail required to set healthy
-    if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
+    // continuous period of 5s without pass required to set unhealthy
-        return true;
+    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_NavEKF3/AP_NavEKF3_core.h

@@ -751,8 +751,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
+    // Assess GPS data quality and set gpsGoodToAlign
-    bool calcGpsGoodToAlign(void);
+    void calcGpsGoodToAlign(void);
 
     // set the class variable true if the delta angle bias variances are sufficiently small
     void checkGyroCalStatus(void);
@@ -905,6 +905,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