ekf2: request mag yaw reset after calibration or sensor change

src/modules/ekf2/EKF/ekf.h

@@ -193,7 +193,8 @@ public:
 	void resetAccelBias();
 
 	// Reset all magnetometer bias states and covariances to initial alignment values.
-	void resetMagBias();
+	// Requests full mag yaw reset (if using mag)
+	void resetMagBiasAndYaw();
 
 	Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -907,7 +907,7 @@ void Ekf::resetAccelBias()
 	_prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag();
 }
 
-void Ekf::resetMagBias()
+void Ekf::resetMagBiasAndYaw()
 {
 	// Zero the magnetometer bias states
 	_state.mag_B.zero();
@@ -918,6 +918,12 @@ void Ekf::resetMagBias()
 
 	// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
 	_saved_mag_bf_variance.zero();
+
+	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+		_mag_yaw_reset_req = true;
+	}
+
+	_control_status.flags.mag_fault = false;
 }
 
 // get EKF innovation consistency check status information comprising of: