AP_NavEKF: Don't allow ground based magnetic anomaly to fail GPS checks

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -4363,9 +4363,6 @@ void NavEKF::readMagData()
         // read compass data and scale to improve numerical conditioning
         magData = _ahrs->get_compass()->get_field() * 0.001f;
 
-        // check for consistent data between magnetometers
-        consistentMagData = _ahrs->get_compass()->consistent();
-
         // get states stored at time closest to measurement time after allowance for measurement delay
         RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - msecMagDelay));
 
@@ -5280,12 +5277,12 @@ bool NavEKF::calcGpsGoodToAlign(void)
         gpsCheckStatus.flags.bad_hAcc = true;
     }
 
-    // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
+    // If we have bad innovations for longer than 5 seconds and are on the ground (disarmed) then 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) {
+    if (magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f) {
         magYawResetTimer_ms = imuSampleTime_ms;
     }
-    if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
+    if ((imuSampleTime_ms - magYawResetTimer_ms > 5000) && !vehicleArmed) {
         // reset heading and field states
         Vector3f eulerAngles;
         getEulerAngles(eulerAngles);

libraries/AP_NavEKF/AP_NavEKF.h

@@ -704,7 +704,6 @@ class NavEKF
     bool gpsAccuracyGood;           // true when the GPS accuracy is considered to be good enough for safe flight.
     uint32_t timeAtDisarm_ms;       // time of last disarm event in msec
     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
     float hgtInnovFiltState;        // state used for fitering of the height innovations used for pre-flight checks
     uint32_t lastConstPosFuseTime_ms;   // last time in msec the constant position constraint was applied
     uint32_t lastGpsAccuracySendTime_ms;    //last sendtime of mavlink GPS_ACCURACY packet