mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 00:04:02 -04:00
AP_NavEKF3: Use specialised function for in-flight plane yaw alignment
This commit is contained in:
parent
4675aea0bf
commit
e3eea0a54a
@ -17,7 +17,7 @@ void NavEKF3_core::controlMagYawReset()
|
|||||||
// Vehicles that can use a zero sideslip assumption (Planes) are a special case
|
// 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
|
// They can use the GPS velocity to recover from bad initial compass data
|
||||||
// This allows recovery for heading alignment errors due to compass faults
|
// 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;
|
gpsYawResetRequest = true;
|
||||||
return;
|
return;
|
||||||
} else {
|
} else {
|
||||||
|
Loading…
Reference in New Issue
Block a user