AP_NavEKF2: Consolidate magnetometer timeout checks

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -255,9 +255,6 @@ void NavEKF2_core::SelectMagFusion()
         return;
     }
 
-    // check for and read new magnetometer measurements
-    readMagData();
-
     // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
     if (magHealth) {
         magTimeout = false;
@@ -266,6 +263,9 @@ void NavEKF2_core::SelectMagFusion()
         magTimeout = true;
     }
 
+    // check for and read new magnetometer measurements
+    readMagData();
+
     // check for availability of magnetometer data to fuse
     magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
 

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -257,6 +257,18 @@ void NavEKF2_core::readMagData()
         InitialiseVariablesMag();
     }
 
+    // If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available
+    // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
+    // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
+    // if the timeout is due to a sensor failure, then declare a timeout regardless of onground status
+    if (maxCount > 1) {
+        bool fusionTimeout = magTimeout && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000;
+        bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
+        if (fusionTimeout || sensorTimeout) {
+            tryChangeCompass();
+        }
+    }
+
     // check for a failed compass and switch if failed for magFailTimeLimit_ms
     if (maxCount > 1 &&
         !compass.healthy(magSelectIndex) &&
@@ -271,12 +283,6 @@ void NavEKF2_core::readMagData()
         compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) {
         frontend->logging.log_compass = true;
 
-        // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
-        // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
-        // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
-        if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
-            tryChangeCompass();
-        }
 
         // detect changes to magnetometer offset parameters and reset states
         Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);