EV yaw: move EV yaw reset out of resetMagHeading function

Also remove the unnecessary 2nd "manual reset" done in the start function

EKF/control.cpp

@@ -222,12 +222,13 @@ void Ekf::controlExternalVisionFusion()
 		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
 			// don't start using EV data unless data is arriving frequently
 			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
-				startEvYawFusion();
+				if (resetYawToEv()) {
+					_control_status.flags.yaw_align = true;
+					startEvYawFusion();
+				}
 			}
 		}
 
-
-
 		// determine if we should use the horizontal position observations
 		if (_control_status.flags.ev_pos) {
 
@@ -342,7 +343,6 @@ void Ekf::controlExternalVisionFusion()
 		stopEvFusion();
 		_warning_events.flags.vision_data_stopped = true;
 		ECL_WARN("vision data stopped");
-
 	}
 }
 

EKF/ekf.h

@@ -673,10 +673,14 @@ private:
 	// update the terrain vertical position estimate using an optical flow measurement
 	void fuseFlowForTerrain();
 
-	// reset the heading and magnetic field states using the declination and magnetometer/external vision measurements
+	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
 	bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true);
 
+	// reset the heading using the external vision measurements
+	// return true if successful
+	bool resetYawToEv();
+
 	// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
 	// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
 	bool realignYawGPS();

EKF/ekf_helper.cpp

@@ -496,14 +496,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	float yaw_new;
 	float yaw_new_variance = 0.0f;
 
-	if (_control_status.flags.ev_yaw) {
-		yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
-
-		if (increase_yaw_var) {
-			yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
-		}
-
-	} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
+	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 		// rotate the magnetometer measurements into earth frame using a zero yaw angle
 		const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
 
@@ -520,7 +513,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 		return true;
 
 	} else {
-		// there is no yaw observation
+		// there is no magnetic yaw observation
 		return false;
 	}
 
@@ -538,6 +531,16 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	return true;
 }
 
+bool Ekf::resetYawToEv()
+{
+	const float yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
+	const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
+
+	resetQuatStateYaw(yaw_new, yaw_new_variance, true);
+
+	return true;
+}
+
 // Return the magnetic declination in radians to be used by the alignment and fusion processing
 float Ekf::getMagDeclination()
 {
@@ -1512,15 +1515,6 @@ void Ekf::startEvVelFusion()
 
 void Ekf::startEvYawFusion()
 {
-	// reset the yaw angle to the value from the vision quaternion
-	const float yaw = getEuler321Yaw(_ev_sample_delayed.quat);
-	const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
-
-	resetQuatStateYaw(yaw, yaw_variance, true);
-
-	// flag the yaw as aligned
-	_control_status.flags.yaw_align = true;
-
 	// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
 	_control_status.flags.ev_yaw = true;
 	_control_status.flags.mag_dec = false;