From 36c946113da327be4a05e37943eeb58dee076df0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 20 Nov 2020 11:48:40 +1100 Subject: [PATCH] AP_NavEKF3: handle compass fallback in yaw source reset if we get to this point we must be using the compass fallback logic, and should do the reset --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index ed7009047f..f9009b7722 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -363,7 +363,8 @@ void NavEKF3_core::SelectMagFusion() // Control reset of yaw and magnetic field states if we are using compass data if (magDataToFuse) { - if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS)) { + if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS || + yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) { magYawResetRequest = true; yaw_source_reset = false; }