ekf2: clear test ratios, flags, etc when stopping mag fusion

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1255,16 +1255,30 @@ void Ekf::stopMag3DFusion()
 	if (_control_status.flags.mag_3D) {
 		saveMagCovData();
 
-		// we are no longer using 3-axis fusion so set the reported test levels to zero
-		_mag_test_ratio.setZero();
-
 		_control_status.flags.mag_3D = false;
+		_control_status.flags.mag_dec = false;
+
+		_mag_innov.zero();
+		_mag_innov_var.zero();
+		_mag_test_ratio.zero();
+
+		_fault_status.flags.bad_mag_x = false;
+		_fault_status.flags.bad_mag_y = false;
+		_fault_status.flags.bad_mag_z = false;
+
+		_fault_status.flags.bad_mag_decl = false;
 	}
 }
 
 void Ekf::stopMagHdgFusion()
 {
-	_control_status.flags.mag_hdg = false;
+	if (_control_status.flags.mag_hdg) {
+		_control_status.flags.mag_hdg = false;
+
+		_fault_status.flags.bad_hdg = false;
+
+		_yaw_test_ratio = 0.f;
+	}
 }
 
 void Ekf::startMagHdgFusion()
@@ -1279,7 +1293,11 @@ void Ekf::startMagHdgFusion()
 void Ekf::startMag3DFusion()
 {
 	if (!_control_status.flags.mag_3D) {
+
 		stopMagHdgFusion();
+
+		_yaw_test_ratio = 0.0f;
+
 		zeroMagCov();
 		loadMagCovData();
 		_control_status.flags.mag_3D = true;

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -195,9 +195,6 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 		}
 	}
 
-	// we are no longer using heading fusion so set the reported test level to zero
-	_yaw_test_ratio = 0.0f;
-
 	// if any axis fails, abort the mag fusion
 	if (!all_innovation_checks_passed) {
 		return false;