From 4904c718a553479d22f7da6d25992b61c31097ed Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 25 Jul 2024 05:42:46 +0930 Subject: [PATCH] AP_NavEKF3: Fix yaw alignment bug When the yaw is aligned to the GPS yaw, the recordYawResetsCompleted() function should be called the same as for any other yaw reset. --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 390dfceb8705a..67ac2a57943a4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -294,6 +294,7 @@ void NavEKF3_core::SelectMagFusion() have_fused_gps_yaw = true; lastSynthYawTime_ms = imuSampleTime_ms; last_gps_yaw_fuse_ms = imuSampleTime_ms; + recordYawResetsCompleted(); } else if (tiltAlignComplete && yawAlignComplete) { have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS); if (have_fused_gps_yaw) {