From c94de61e2957820a6e30297142fce7e548353343 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 4 May 2020 08:52:56 +1000 Subject: [PATCH] AP_NavEKF2: Don't perform emergency yaw reset unless commanded externally This limits the use of the reset to situations where it is a last ditch resort before a lane switch and failsafe. This will limit false positives for general deployment, but still provide protection from fly-aways at the cost of some increase in reaction time. --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 4c34d9209df..2e7db0b23ac 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -234,7 +234,6 @@ void NavEKF2_core::setAidingMode() // Check if the loss of position accuracy has become critical bool posAidLossCritical = false; - bool posAidLossPending = false; if (!posAiding ) { uint16_t maxLossTime_ms; if (!velAiding) { @@ -245,9 +244,6 @@ void NavEKF2_core::setAidingMode() posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && (imuSampleTime_ms - lastExtNavPassTime_ms > maxLossTime_ms) && (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); - posAidLossPending = (imuSampleTime_ms - lastRngBcnPassTime_ms > (uint32_t)frontend->_gsfResetDelay) && - (imuSampleTime_ms - lastExtNavPassTime_ms > (uint32_t)frontend->_gsfResetDelay) && - (imuSampleTime_ms - lastPosPassTime_ms > (uint32_t)frontend->_gsfResetDelay); } if (attAidLossCritical) { @@ -267,9 +263,6 @@ void NavEKF2_core::setAidingMode() velTimeout = true; rngBcnTimeout = true; gpsNotAvailable = true; - } else if (posAidLossPending) { - // attempt to reset the yaw to the estimate from the EKF-GSF algorithm - EKFGSF_yaw_reset_request_ms = imuSampleTime_ms; } break; }