AP_NavEKF3: Use specialised function for in-flight plane yaw alignment

This commit is contained in:
Paul Riseborough 2021-02-28 14:41:29 +11:00 committed by Andrew Tridgell
parent 4675aea0bf
commit e3eea0a54a

View File

@ -17,7 +17,7 @@ void NavEKF3_core::controlMagYawReset()
// Vehicles that can use a zero sideslip assumption (Planes) are a special case
// They can use the GPS velocity to recover from bad initial compass data
// This allows recovery for heading alignment errors due to compass faults
if (assume_zero_sideslip() && !finalInflightYawInit && inFlight) {
if (assume_zero_sideslip() && (!finalInflightYawInit || !yawAlignComplete) && inFlight) {
gpsYawResetRequest = true;
return;
} else {