From 3836b5904198bfc1942224f87e53de2c901b929e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 28 Aug 2020 12:42:42 +1000 Subject: [PATCH] AP_NavEKF3: 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_NavEKF3/AP_NavEKF3_Measurements.cpp | 85 ++++++++++++------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 5 +- 2 files changed, 59 insertions(+), 31 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 26e2865e61..671f63c654 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -248,6 +248,41 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f * MAGNETOMETER * ********************************************************/ +// try changing compass, return true if a new compass is found +void NavEKF3_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, "EKF3 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 NavEKF3_core::readMagData() { @@ -260,7 +295,7 @@ void NavEKF3_core::readMagData() // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable // magnetometer, then declare the magnetometers as failed for this flight - uint8_t maxCount = compass.get_count(); + const uint8_t maxCount = compass.get_count(); if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) { allMagSensorsFailed = true; return; @@ -277,9 +312,18 @@ void NavEKF3_core::readMagData() yawAlignComplete = false; 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(); + } + // limit compass update rate 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) > 1000 * frontend->sensorIntervalMin_ms)) { + if (use_compass() && + compass.healthy(magSelectIndex) && + ((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) { frontend->logging.log_compass = true; // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available @@ -290,33 +334,8 @@ void NavEKF3_core::readMagData() imuSampleTime_ms - ekfStartTime_ms > 30000 && !(frontend->_affinity & EKF_AFFINITY_MAG)) { - // 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, "EKF3 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; - } - } + // this compass has timed out (innovations too large for magFailTimeLimit_ms), try a new compass + tryChangeCompass(); } // detect changes to magnetometer offset parameters and reset states @@ -334,6 +353,9 @@ void NavEKF3_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; @@ -348,6 +370,9 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index c6b35a5c8c..ec5fe6b572 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -772,6 +772,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(); @@ -1047,6 +1050,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 @@ -1108,7 +1112,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