Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2020-08-28 12:42:42 +10:00
parent 6b9e595602
commit 3836b59041
2 changed files with 59 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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; 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, "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;
@ -278,8 +313,17 @@ void NavEKF3_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();
}
// 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; 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, "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;
}
}

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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