From 9afc3905525948d4b8647fe30f4822d78723a4e0 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 25 Oct 2021 14:01:08 +0200
Subject: [PATCH] ekf2: move gps yaw reset in starting function

---
 src/modules/ekf2/EKF/control.cpp    |  6 +++---
 src/modules/ekf2/EKF/ekf_helper.cpp | 14 +++++++++-----
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index 0922c4d9cf..fd125244a4 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -820,10 +820,10 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 		} else {
 			if (starting_conditions_passing) {
 				// Try to activate GPS yaw fusion
-				if (resetYawToGps()) {
-					_control_status.flags.yaw_align = true;
+				startGpsYawFusion();
+
+				if (_control_status.flags.gps_yaw) {
 					_nb_gps_yaw_reset_available = 1;
-					startGpsYawFusion();
 				}
 			}
 		}
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index a08c67498e..8081389df4 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -1546,11 +1546,15 @@ void Ekf::stopGpsVelFusion()
 
 void Ekf::startGpsYawFusion()
 {
-	_control_status.flags.mag_dec = false;
-	stopEvYawFusion();
-	stopMagHdgFusion();
-	stopMag3DFusion();
-	_control_status.flags.gps_yaw = true;
+	if (resetYawToGps()) {
+		_control_status.flags.yaw_align = true;
+		_control_status.flags.mag_dec = false;
+		stopEvYawFusion();
+		stopMagHdgFusion();
+		stopMag3DFusion();
+		_control_status.flags.gps_yaw = true;
+	}
+
 }
 
 void Ekf::stopGpsYawFusion()