AP_NavEKF2: Bring pre-flight GPS checks up to date with EKF1

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -3880,6 +3880,9 @@ void NavEKF2_core::readMagData()
         // read compass data and scale to improve numerical conditioning
         magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
 
+        // check for consistent data between magnetometers
+        consistentMagData = _ahrs->get_compass()->consistent();
+
         // check if compass offsets have been changed and adjust EKF bias states to maintain consistent innovations
         if (_ahrs->get_compass()->healthy(0)) {
             Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
@@ -4168,6 +4171,7 @@ void NavEKF2_core::InitialiseVariables()
     hgtMeasTime_ms = imuSampleTime_ms;
     magMeasTime_ms = imuSampleTime_ms;
     timeTasReceived_ms = 0;
+    magYawResetTimer_ms = imuSampleTime_ms;
 
     // initialise other variables
     gpsNoiseScaler = 1.0f;
@@ -4661,9 +4665,13 @@ void NavEKF2_core::setTouchdownExpected(bool val)
     expectGndEffectTouchdown = val;
 }
 
-// Monitor GPS data to see if quality is good enough to initialise the EKF
-// Monitor magnetometer innovations to to see if the heading is good enough to use GPS
-// Return true if all criteria pass for 10 seconds
+/* Monitor GPS data to see if quality is good enough to initialise the EKF
+   Monitor magnetometer innovations to to see if the heading is good enough to use GPS
+   Return true if all criteria pass for 10 seconds
+
+   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 NavEKF2_core::calcGpsGoodToAlign(void)
 {
     // calculate absolute difference between GPS vert vel and inertial vert vel
@@ -4673,10 +4681,21 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
     } else {
         velDiffAbs = 0.0f;
     }
+
     // fail if velocity difference or reported speed accuracy greater than threshold
     bool gpsVelFail = (velDiffAbs > 1.0f) || (gpsSpdAccuracy > 1.0f);
-    // fail if not enough sats
-    bool numSatsFail = _ahrs->get_gps().num_sats() < 6;
+
+    if (velDiffAbs > 1.0f) {
+        hal.util->snprintf(prearm_fail_string,
+                           sizeof(prearm_fail_string),
+                           "GPS vert vel error %.1f", (double)velDiffAbs);
+    }
+    if (gpsSpdAccuracy > 1.0f) {
+        hal.util->snprintf(prearm_fail_string,
+                           sizeof(prearm_fail_string),
+                           "GPS speed error %.1f", (double)gpsSpdAccuracy);
+    }
+
     // fail if horiziontal position accuracy not sufficient
     float hAcc = 0.0f;
     bool hAccFail;
@@ -4685,6 +4704,26 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
     } else {
         hAccFail =  false;
     }
+    if (hAccFail) {
+        hal.util->snprintf(prearm_fail_string,
+                           sizeof(prearm_fail_string),
+                           "GPS horiz error %.1f", (double)hAcc);
+    }
+
+    // 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) || !consistentMagData) {
+        magYawResetTimer_ms = imuSampleTime_ms;
+    }
+    if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
+        // reset heading and field states
+        Vector3f eulerAngles;
+        getEulerAngles(eulerAngles);
+        stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+        // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
+        magYawResetTimer_ms = imuSampleTime_ms;
+    }
+
     // fail if magnetometer innovations are outside limits indicating bad yaw
     // with bad yaw we are unable to use GPS
     bool yawFail;
@@ -4693,17 +4732,42 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
     } else {
         yawFail = false;
     }
-    // record time of fail
-    // assume  fail first time called
-    if (gpsVelFail || numSatsFail || hAccFail || yawFail || lastGpsVelFail_ms == 0) {
+    if (yawFail) {
+        hal.util->snprintf(prearm_fail_string,
+                           sizeof(prearm_fail_string),
+                           "Mag yaw error x=%.1f y=%.1f",
+                           (double)magTestRatio.x,
+                           (double)magTestRatio.y);
+    }
+
+    // fail if not enough sats
+    bool numSatsFail = _ahrs->get_gps().num_sats() < 6;
+    if (numSatsFail) {
+        hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
+                           "GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
+    }
+
+    // record time of fail if failing
+    if (gpsVelFail || numSatsFail || hAccFail || yawFail) {
+        lastGpsVelFail_ms = imuSampleTime_ms;
+    }
+
+    if (lastGpsVelFail_ms == 0) {
+        // first time through, start with a failure
         lastGpsVelFail_ms = imuSampleTime_ms;
+        hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF warmup");
     }
+
+    // DEBUG PRINT
+    //hal.console->printf("velDiff = %5.2f, nSats = %i, hAcc = %5.2f, sAcc = %5.2f, delTime = %i\n", velDiffAbs, _ahrs->get_gps().num_sats(), hAcc, gpsSpdAccuracy, imuSampleTime_ms - lastGpsVelFail_ms);
     // continuous period without fail required to return healthy
+
     if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
+        // we have not failed in the last 10 seconds
         return true;
-    } else {
-        return false;
     }
+
+    return false;
 }
 
 // Read the range finder and take new measurements if available

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -699,6 +699,8 @@ private:
     float innovYaw;                 // compass yaw angle innovation (rad)
     uint32_t timeTasReceived_ms;    // tie last TAS data was received (msec)
     bool gpsQualGood;               // true when the GPS quality can be used to initialise the navigation system
+    uint32_t magYawResetTimer_ms;   // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
+    bool consistentMagData;         // true when the magnetometers are passing consistency checks
 
     // variables added for optical flow fusion
     of_elements storedOF[OBS_BUFFER_LENGTH];    // OF data buffer
@@ -799,6 +801,9 @@ private:
     } mag_state;
 
 
+    // string representing last reason for prearm failure
+    char prearm_fail_string[40];
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // performance counters
     perf_counter_t  _perf_UpdateFilter;