AP_NavEKF2: fixed handling of failed compass

when a compass goes unhealthy due to sensor failure we should try
another compass after 10s if another compass is available

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -192,6 +192,41 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
 *                      MAGNETOMETER                     *
 ********************************************************/
 
+// try changing to another compass
+void NavEKF2_core::tryChangeCompass(void)
+{
+    const Compass &compass = AP::compass();
+    const uint8_t maxCount = compass.get_count();
+
+    // search through the list of magnetometers
+    for (uint8_t i=1; i<maxCount; i++) {
+        uint8_t tempIndex = magSelectIndex + i;
+        // loop back to the start index if we have exceeded the bounds
+        if (tempIndex >= maxCount) {
+            tempIndex -= maxCount;
+        }
+        // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
+        if (compass.healthy(tempIndex) && compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
+            magSelectIndex = tempIndex;
+            gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
+            // reset the timeout flag and timer
+            magTimeout = false;
+            lastHealthyMagTime_ms = imuSampleTime_ms;
+            // zero the learned magnetometer bias states
+            stateStruct.body_magfield.zero();
+            // clear the measurement buffer
+            storedMag.reset();
+            // clear the data waiting flag so that we do not use any data pending from the previous sensor
+            magDataToFuse = false;
+            // request a reset of the magnetic field states
+            magStateResetRequest = true;
+            // declare the field unlearned so that the reset request will be obeyed
+            magFieldLearned = false;
+            return;
+        }
+    }
+}
+
 // check for new magnetometer data and update store measurements if available
 void NavEKF2_core::readMagData()
 {
@@ -222,44 +257,25 @@ void NavEKF2_core::readMagData()
         InitialiseVariablesMag();
     }
 
+    // check for a failed compass and switch if failed for magFailTimeLimit_ms
+    if (maxCount > 1 &&
+        !compass.healthy(magSelectIndex) &&
+        imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms) {
+        tryChangeCompass();
+    }
     
     // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
     // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
-    if (use_compass() && compass.last_update_usec() - lastMagUpdate_us > 70000) {
+    if (use_compass() &&
+        compass.healthy(magSelectIndex) &&
+        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) {
-
-            // search through the list of magnetometers
-            for (uint8_t i=1; i<maxCount; i++) {
-                uint8_t tempIndex = magSelectIndex + i;
-                // loop back to the start index if we have exceeded the bounds
-                if (tempIndex >= maxCount) {
-                    tempIndex -= maxCount;
-                }
-                // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
-                if (compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
-                    magSelectIndex = tempIndex;
-                    gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
-                    // reset the timeout flag and timer
-                    magTimeout = false;
-                    lastHealthyMagTime_ms = imuSampleTime_ms;
-                    // zero the learned magnetometer bias states
-                    stateStruct.body_magfield.zero();
-                    // clear the measurement buffer
-                    storedMag.reset();
-                    // clear the data waiting flag so that we do not use any data pending from the previous sensor
-                    magDataToFuse = false;
-                    // request a reset of the magnetic field states
-                    magStateResetRequest = true;
-                    // declare the field unlearned so that the reset request will be obeyed
-                    magFieldLearned = false;
-                    break;
-                }
-            }
+            tryChangeCompass();
         }
 
         // detect changes to magnetometer offset parameters and reset states
@@ -277,6 +293,9 @@ void NavEKF2_core::readMagData()
         // store time of last measurement update
         lastMagUpdate_us = compass.last_update_usec(magSelectIndex);
 
+        // Magnetometer data at the current time horizon
+        mag_elements magDataNew;
+
         // estimate of time magnetometer measurement was taken, allowing for delays
         magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
 
@@ -291,6 +310,9 @@ void NavEKF2_core::readMagData()
 
         // save magnetometer measurement to buffer to be fused later
         storedMag.push(magDataNew);
+
+        // remember time we read compass, to detect compass sensor failure
+        lastMagRead_ms = imuSampleTime_ms;
     }
 }
 

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -651,6 +651,9 @@ private:
     // check for new magnetometer data and update store measurements if available
     void readMagData();
 
+    // try changing compasses on compass failure or timeout
+    void tryChangeCompass(void);
+
     // check for new airspeed data and update stored measurements if available
     void readAirSpdData();
 
@@ -905,6 +908,7 @@ private:
     uint32_t prevTasStep_ms;        // time stamp of last TAS fusion step
     uint32_t prevBetaStep_ms;       // time stamp of last synthetic sideslip fusion step
     uint32_t lastMagUpdate_us;      // last time compass was updated in usec
+    uint32_t lastMagRead_ms;        // last time compass data was successfully read
     Vector3f velDotNED;             // rate of change of velocity in NED frame
     Vector3f velDotNEDfilt;         // low pass filtered velDotNED
     uint32_t imuSampleTime_ms;      // time that the last IMU value was taken
@@ -967,7 +971,6 @@ private:
     tas_elements tasDataNew;        // TAS data at the current time horizon
     tas_elements tasDataDelayed;    // TAS data at the fusion time horizon
     uint8_t tasStoreIndex;          // TAS data storage index
-    mag_elements magDataNew;        // Magnetometer data at the current time horizon
     mag_elements magDataDelayed;    // Magnetometer data at the fusion time horizon
     uint8_t magStoreIndex;          // Magnetometer data storage index
     gps_elements gpsDataNew;        // GPS data at the current time horizon