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

@@ -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();
-				}
+				_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

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{};
 

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 {