From 14268ad3e3b15c8b217c1472881ddc1beedac5eb Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 22 Sep 2016 08:08:59 +1000 Subject: [PATCH] AP_NavEKF: Ensure in-flight yaw alignment is performed if over-written Landing on a surface with a magnetic anomaly large enough to require the EKF to reset its field states can result in poor heading and large EKF magnetometer and velocity innovations if the vehicle is re-armed and takes off. This happens because the flags indicating that completion of the in-air yaw and field states was not being reset if a ground reset had occurred. This patch ensures that any reset of magnetic field states that occurs whilst disarmed causes the flags to be reset. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 0736e8bde8..a30ede5fd6 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4529,6 +4529,12 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch) yawAligned = false; } + if (!vehicleArmed) { + // The flags indicating that the in-air alignment has been completed need to be cleared + // because this alignment is on-ground and the yaw and field values could be incorrect. + secondMagYawInit = firstMagYawInit = false; + } + // return attitude quaternion return initQuat; }