Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: fixed handling of failed compass

Browse Source 
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 c2feebea13
commit 6b9e595602
2 changed files with 55 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

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

5
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

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