Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix bug causing too frequent resets if bad IMU data detected

Browse Source
This commit is contained in:
Paul Riseborough 2021-09-19 12:57:01 +10:00 committed by Andrew Tridgell
parent f0efc1300e
commit 17c2ba5ac1
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1220,11 +1220,13 @@ void NavEKF3_core::selectHeightForFusion()
ResetPositionD(-hgtMea);
}
// If we haven't fused height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
// If we haven't fused height data for a while or have bad IMU data, then declare the height data as being timed out
// set height timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms ||
(badIMUdata && (imuSampleTime_ms - goodIMUdata_ms < BAD_IMU_DATA_TIMEOUT_MS))) {
(badIMUdata &&
(imuSampleTime_ms - goodIMUdata_ms > BAD_IMU_DATA_TIMEOUT_MS) &&
(imuSampleTime_ms - lastPosResetD_ms > BAD_IMU_DATA_TIMEOUT_MS))) {
hgtTimeout = true;
} else {
hgtTimeout = false;