From 17c2ba5ac195c2e1956f6c54fa7bddc28207921c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 19 Sep 2021 12:57:01 +1000 Subject: [PATCH] AP_NavEKF3: Fix bug causing too frequent resets if bad IMU data detected --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 081e40e21f..df41a0ac7f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -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;