diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 61aaad4df1..b386c45b61 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -57,6 +57,13 @@ void NavEKF2_core::controlMagYawReset() } } + // In-Flight yaw alignment for vehicles that can use a zero sideslip assumption (Planes) + // and are not using a compass + if (!yawAlignComplete && assume_zero_sideslip() && inFlight) { + realignYawGPS(); + firstMagYawInit = yawAlignComplete; + } + // In-Flight reset for vehicles that can use a zero sideslip assumption (Planes) // this is done to protect against unrecoverable heading alignment errors due to compass faults if (!firstMagYawInit && assume_zero_sideslip() && inFlight) {