From 7a8783f35ead83a6a908a3f40a948e265ee6b317 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 4 Mar 2017 21:44:19 +1100 Subject: [PATCH] AP_NavEKF3: Fix bug preventing reset to GPS This fixes a bug that prevented the reset to the GPS position occurring if GPS velocity observations were still passing innovation consistency checks. --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 3578743a7c..794270c638 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -252,8 +252,7 @@ void NavEKF3_core::setAidingMode() maxLossTime_ms = frontend->posRetryTimeUseVel_ms; } posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && - (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms) && - (imuSampleTime_ms - lastVelPassTime_ms > maxLossTime_ms); + (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); } if (attAidLossCritical) {