From c5d2620206974d119266d0ec25760b43c56516d1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 27 Jun 2020 18:50:10 +1000 Subject: [PATCH] AP_NavEKF3: Don't delay GPS use unnecessarily Fixed wing should not wait for bias state convergence after in-flight yaw alignment --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index df9b19c7a8084a..56a3d28b61dc0f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -460,7 +460,7 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const // return true if the filter to be ready to use gps bool NavEKF3_core::readyToUseGPS(void) const { - return validOrigin && tiltAlignComplete && yawAlignComplete && delAngBiasLearned && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit; + return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit; } // return true if the filter to be ready to use the beacon range measurements @@ -552,8 +552,8 @@ void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); - if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW)) { - // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass + if (!use_compass() && (effectiveMagCal != MagCal::EXTERNAL_YAW) && (effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK)) { + // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference // which can make this check fail Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]); Vector3f temp = prevTnb * delAngBiasVarVec;