EKF: Fix bug when resetting position and velocities for fw due to something else than bad yaw estimate

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>

EKF/control.cpp

@@ -454,6 +454,8 @@ void Ekf::controlGpsFusion()
 				if (_control_status.flags.fixed_wing) {
 					// if flying a fixed wing aircraft, do a complete reset that includes yaw, velocity and position
 					realignYawGPS();
+					resetVelocity();
+					resetPosition();
 				} else {
 					resetVelocity();
 					resetPosition();

EKF/ekf_helper.cpp

@@ -414,10 +414,6 @@ bool Ekf::realignYawGPS()
 			// update transformation matrix from body to world frame using the current state estimate
 			_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
 
-			// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
-			resetVelocity();
-			resetPosition();
-
 			// Use the last magnetometer measurements to reset the field states
 			_state.mag_B.zero();
 			_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;