From 6b9e5956024f200bca975a77fb180bcd55bf73b0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 28 Aug 2020 12:42:42 +1000 Subject: [PATCH] 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 --- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 80 ++++++++++++------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 5 +- 2 files changed, 55 insertions(+), 30 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 729846f9e0..2a8c4aaec2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/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) { + 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) { - 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; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index ef225884e8..a9684e0ab6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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