Heading reset to magnetometer from GPS or EV (#969)

EKF/control.cpp

@@ -333,7 +333,7 @@ void Ekf::controlExternalVisionFusion()
 			fuseHeading();
 		}
 
-	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
+	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel ||  _control_status.flags.ev_yaw)
 		   && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
 
 		// Turn off EV fusion mode if no data has been received

EKF/ekf.h

@@ -382,6 +382,7 @@ private:
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
 	bool _synthetic_mag_z_active{false};	///< true if we are generating synthetic magnetometer Z measurements
+	bool _non_mag_yaw_aiding_running_prev{false};  ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
 
 	bool _is_yaw_fusion_inhibited{false};		///< true when yaw sensor use is being inhibited
 
@@ -778,6 +779,7 @@ private:
 	void controlMagFusion();
 
 	bool noOtherYawAidingThanMag() const;
+	bool otherHeadingSourcesHaveStopped();
 
 	void checkHaglYawResetReq();
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }

EKF/mag_control.cpp

@@ -72,6 +72,8 @@ void Ekf::controlMagFusion()
 		return;
 	}
 
+	_mag_yaw_reset_req |= otherHeadingSourcesHaveStopped();
+
 	if (noOtherYawAidingThanMag() && _mag_data_ready) {
 		if (_control_status.flags.in_air) {
 			checkHaglYawResetReq();
@@ -327,3 +329,12 @@ void Ekf::run3DMagAndDeclFusions()
 	}
 }
 
+bool Ekf::otherHeadingSourcesHaveStopped()
+{
+    // detect rising edge of noOtherYawAidingThanMag()
+    bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
+
+    _non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
+
+    return  result;
+}