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

AP_NavEKF3: use same mag switch pattern as EKF2

Browse Source 
based on Pauls suggested change
This commit is contained in:
Andrew Tridgell 2020-09-07 13:12:58 +10:00
parent da7bb44a61
commit c03c43e52c
2 changed files with 17 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -358,13 +358,6 @@ void NavEKF3_core::SelectMagFusion()
// fall through to magnetometer fusion
}
if (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) {
// check for and read new magnetometer measurements. We don't
// real for EXTERNAL_YAW_FALLBACK as it has already been read
// above
readMagData();
}
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
if (magHealth) {
magTimeout = false;
@ -373,6 +366,13 @@ void NavEKF3_core::SelectMagFusion()
magTimeout = true;
}
if (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) {
// check for and read new magnetometer measurements. We don't
// real for EXTERNAL_YAW_FALLBACK as it has already been read
// above
readMagData();
}
// check for availability of magnetometer or other yaw data to fuse
magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);

27
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -313,11 +313,16 @@ 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();
// 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 && !(frontend->_affinity & EKF_AFFINITY_MAG);
bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
if (fusionTimeout || sensorTimeout) {
tryChangeCompass();
}
}
// limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
@ -326,18 +331,6 @@ void NavEKF3_core::readMagData()
((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
// 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 &&
!(frontend->_affinity & EKF_AFFINITY_MAG)) {
// 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
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);