AP_NavEKF2: Handle case where we are in-flight and haven't yet aligned the yaw
This happens if planes are flown without a compass
This commit is contained in:
parent
581c1aa0d4
commit
1179c08473
@ -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) {
|
||||
|
Loading…
Reference in New Issue
Block a user