From c81cdfa1cefc64b848a85e444df47d15093bace4 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Tue, 17 Oct 2017 15:08:09 +0200 Subject: [PATCH] EKF: Fix bug when resetting position and velocities for fw due to something else than bad yaw estimate Signed-off-by: CarlOlsson --- EKF/control.cpp | 2 ++ EKF/ekf_helper.cpp | 4 ---- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index d4690753e3..d27421b831 100644 --- a/EKF/control.cpp +++ b/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(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8277ba0fab..524cb7ffd5 100644 --- a/EKF/ekf_helper.cpp +++ b/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;