aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Consolidate magnetometer timeout checks

Browse Source
This commit is contained in:
Paul Riseborough 2020-08-28 21:52:38 +10:00 committed by Andrew Tridgell
parent 37866ec3cf
commit da7bb44a61
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -255,9 +255,6 @@ void NavEKF2_core::SelectMagFusion()
return;
}
// check for and read new magnetometer measurements
readMagData();
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
if (magHealth) {
magTimeout = false;
@ -266,6 +263,9 @@ void NavEKF2_core::SelectMagFusion()
magTimeout = true;
}
// check for and read new magnetometer measurements
readMagData();
// check for availability of magnetometer data to fuse
magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);

18
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -257,6 +257,18 @@ void NavEKF2_core::readMagData()
InitialiseVariablesMag();
}
// If the magnetometer has timed out (been rejected for 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 the timeout is due to a sensor failure, then declare a timeout regardless of onground status
if (maxCount > 1) {
bool fusionTimeout = magTimeout && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000;
bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
if (fusionTimeout || sensorTimeout) {
tryChangeCompass();
}
}
// check for a failed compass and switch if failed for magFailTimeLimit_ms
if (maxCount > 1 &&
!compass.healthy(magSelectIndex) &&
@ -271,12 +283,6 @@ void NavEKF2_core::readMagData()
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) {
tryChangeCompass();
}
// detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);