Skip to content

Commit 36c9461

Browse files
tridgermackay9
authored andcommitted
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
1 parent f257dc2 commit 36c9461

File tree

1 file changed

+2
-1
lines changed

1 file changed

+2
-1
lines changed

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -363,7 +363,8 @@ void NavEKF3_core::SelectMagFusion()
363363

364364
// Control reset of yaw and magnetic field states if we are using compass data
365365
if (magDataToFuse) {
366-
if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS)) {
366+
if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS ||
367+
yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
367368
magYawResetRequest = true;
368369
yaw_source_reset = false;
369370
}

0 commit comments

Comments
 (0)