From 363edf5eb9124777b510e601874b907a7ef30b13 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 9 Nov 2017 10:16:48 +1100
Subject: [PATCH] EKF: Fix yaw reset for fixed wing

Ensures that a complete reset of velocity and position states will always be performed if yaw has had to be reset using GPS velocity.
Ensures that the yaw_align status cannot be set to false once the filter has aligned.
---
 EKF/control.cpp    | 24 +++++++++++++-----------
 EKF/ekf.h          |  1 +
 EKF/ekf_helper.cpp |  5 +++--
 3 files changed, 17 insertions(+), 13 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index f386e1a04c..759468659c 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -485,14 +485,12 @@ void Ekf::controlGpsFusion()
 			if (do_reset) {
 				// Reset states to the last GPS measurement
 				if (_control_status.flags.fixed_wing) {
-					// if flying a fixed wing aircraft, do a complete reset that includes yaw, velocity and position
+					// if flying a fixed wing aircraft, do a complete reset that includes yaw
 					realignYawGPS();
-					resetVelocity();
-					resetPosition();
-				} else {
-					resetVelocity();
-					resetPosition();
 				}
+				resetVelocity();
+				resetPosition();
+				_velpos_reset_request = false;
 				ECL_WARN("EKF GPS fusion timeout - reset to GPS");
 
 				// Reset the timeout counters
@@ -1200,12 +1198,16 @@ void Ekf::controlMagFusion()
 					if (!_flt_mag_align_complete) {
 						// If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading
 						if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
-							_control_status.flags.yaw_align = realignYawGPS();
-							_flt_mag_align_complete = _control_status.flags.yaw_align;
+							_flt_mag_align_complete = realignYawGPS();
+							if (_velpos_reset_request) {
+								resetVelocity();
+								resetPosition();
+								_velpos_reset_request = false;
+							}
 						} else {
-							_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
-							_flt_mag_align_complete = _control_status.flags.yaw_align;
+							_flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
 						}
+						_control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete;
 					} else {
 						// reset the mag field covariances
 						zeroRows(P, 16, 21);
@@ -1261,7 +1263,7 @@ void Ekf::controlMagFusion()
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
 			// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
 			if (!_control_status.flags.mag_3D) {
-				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag) || _control_status.flags.yaw_align;
 			}
 
 			// always use 3-axis mag fusion
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 2309748ff4..db130e2bea 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -380,6 +380,7 @@ private:
 	bool _flt_mag_align_complete{true};	///< true when the in-flight mag field alignment has been completed
 	uint64_t _time_last_movement{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	float _saved_mag_variance[6] {};	///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
+	bool _velpos_reset_request{false};	///< true when a large yaw error has been fixed and a velocity and position state reset is required
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 65ddc59ade..5b9a9c0371 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -411,6 +411,9 @@ bool Ekf::realignYawGPS()
 			// calculate new filter quaternion states using corected yaw angle
 			_state.quat_nominal = Quatf(euler321);
 
+			// If heading was bad, then we alos need to reset the velocity and position states
+			_velpos_reset_request = badMagYaw;
+
 			// update transformation matrix from body to world frame using the current state estimate
 			_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
 
@@ -453,8 +456,6 @@ bool Ekf::realignYawGPS()
 			// capture the reset event
 			_state_reset_status.quat_counter++;
 
-			// the alignment using GPS has been successful
-			_control_status.flags.yaw_align = true;
 			return true;
 
 		} else {