From fcccdcc936ba4bd64c559181cb9358af7d695f1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Oct 2020 14:59:19 +1100 Subject: [PATCH] AP_NavEKF3: fixed bug in external yaw for fixed wing when we are on the ground we should not chang to AID_NONE if we have an external yaw source this fixes an EKF3 error loop on the ground found by Michael --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 712cfa8a53..a0294a776d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -289,7 +289,7 @@ void NavEKF3_core::setAidingMode() // This is a special case for when we are a fixed wing aircraft vehicle that has landed and // has no yaw measurement that works when static. Declare the yaw as unaligned (unknown) // and declare attitude aiding loss so that we fall back into a non-aiding mode - if (assume_zero_sideslip() && onGround && !use_compass()){ + if (assume_zero_sideslip() && onGround && !use_compass() && !using_external_yaw()) { yawAlignComplete = false; finalInflightYawInit = false; attAidLossCritical = true;