ekf2: move gps yaw reset in starting function

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()) {
+				startGpsYawFusion();
-					_control_status.flags.yaw_align = true;
+
+				if (_control_status.flags.gps_yaw) {
 					_nb_gps_yaw_reset_available = 1;
-					startGpsYawFusion();
 				}
 			}
 		}

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1546,11 +1546,15 @@ void Ekf::stopGpsVelFusion()
 
 void Ekf::startGpsYawFusion()
 {
-	_control_status.flags.mag_dec = false;
+	if (resetYawToGps()) {
-	stopEvYawFusion();
+		_control_status.flags.yaw_align = true;
-	stopMagHdgFusion();
+		_control_status.flags.mag_dec = false;
-	stopMag3DFusion();
+		stopEvYawFusion();
-	_control_status.flags.gps_yaw = true;
+		stopMagHdgFusion();
+		stopMag3DFusion();
+		_control_status.flags.gps_yaw = true;
+	}
+
 }
 
 void Ekf::stopGpsYawFusion()