ekf2 mag: clear mag_fault when healthy again

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -168,7 +168,13 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		checkMagHeadingConsistency(mag_sample);
 
+		if (_control_status.flags.mag_fault && _control_status.flags.mag_heading_consistent
+		    && isTimedOut(_time_last_heading_fuse, _params.reset_timeout_max)) {
+			_control_status.flags.mag_fault = false;
+		}
+
 		{
+
 			const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !isNorthEastAidingActive();
 			const bool common_conditions_passing = _control_status.flags.mag
 							       && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
@@ -188,8 +194,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 							    || (_params.ekf2_mag_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
 		}
 
-		// TODO: allow clearing mag_fault if mag_3d is good?
-
 		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
 			ECL_INFO("starting mag 3D fusion");
 
@@ -206,10 +210,18 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
 
-				if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D
+				if (checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D
-								|| _control_status.flags.yaw_manual))
+							       || _control_status.flags.yaw_manual)) {
-				    || (wmm_updated && no_ne_aiding_or_not_moving)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
+					const bool reset_heading = ((_control_status.flags.mag_hdg || _control_status.flags.mag_3D) && !isNorthEastAidingActive());
+					resetMagStates(_mag_lpf.getState(), reset_heading);
+
+					// record the start time for the magnetic field alignment
+					_control_status.flags.mag_aligned_in_flight = true;
+					_flt_mag_align_start_time = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
+
+				} else if (wmm_updated && no_ne_aiding_or_not_moving) {
 					const bool reset_heading = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
 					resetMagStates(_mag_lpf.getState(), reset_heading);
 					aid_src.time_last_fuse = imu_sample.time_us;
@@ -443,12 +455,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 			 (double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
 			 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2));
 	}
-
-	// record the start time for the magnetic field alignment
-	if (_control_status.flags.in_air && (reset_heading || _control_status.flags.yaw_manual)) {
-		_control_status.flags.mag_aligned_in_flight = true;
-		_flt_mag_align_start_time = _time_delayed_us;
-	}
 }
 
 void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1263,10 +1263,15 @@ void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const
 
 float Ekf::getHeadingInnov() const
 {
+	float innov = 0.f;
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 
 	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		return Vector3f(_aid_src_mag.innovation).max();
+		innov = Vector3f(_aid_src_mag.innovation).max();
+
+	} else {
+		innov = _mag_heading_innov_lpf.getState();
 	}
 
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -1274,7 +1279,7 @@ float Ekf::getHeadingInnov() const
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
 	if (_control_status.flags.gnss_yaw) {
-		return _aid_src_gnss_yaw.innovation;
+		innov = _aid_src_gnss_yaw.innovation;
 	}
 
 #endif // CONFIG_EKF2_GNSS_YAW
@@ -1282,12 +1287,12 @@ float Ekf::getHeadingInnov() const
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 
 	if (_control_status.flags.ev_yaw) {
-		return _aid_src_ev_yaw.innovation;
+		innov = _aid_src_ev_yaw.innovation;
 	}
 
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	return 0.f;
+	return innov;
 }
 
 float Ekf::getHeadingInnovVar() const

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -210,6 +210,11 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
 	return _ekf->control_status_flags().ev_yaw;
 }
 
+bool EkfWrapper::isIntendingMagFusion() const
+{
+	return _ekf->control_status_flags().mag;
+}
+
 bool EkfWrapper::isIntendingMagHeadingFusion() const
 {
 	return _ekf->control_status_flags().mag_hdg;
@@ -225,6 +230,11 @@ bool EkfWrapper::isMagHeadingConsistent() const
 	return _ekf->control_status_flags().mag_heading_consistent;
 }
 
+bool EkfWrapper::isMagFaultDetected() const
+{
+	return _ekf->control_status_flags().mag_fault;
+}
+
 void EkfWrapper::setMagFuseTypeNone()
 {
 	_ekf_params->ekf2_mag_type = MagFuseType::NONE;

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -102,9 +102,11 @@ public:
 	void disableExternalVisionHeadingFusion();
 	bool isIntendingExternalVisionHeadingFusion() const;
 
+	bool isIntendingMagFusion() const;
 	bool isIntendingMagHeadingFusion() const;
 	bool isIntendingMag3DFusion() const;
 	bool isMagHeadingConsistent() const;
+	bool isMagFaultDetected() const;
 	void setMagFuseTypeNone();
 	void enableMagStrengthCheck();
 	void enableMagInclinationCheck();

src/modules/ekf2/test/sensor_simulator/mag.cpp

@@ -15,7 +15,7 @@ Mag::~Mag()
 
 void Mag::send(uint64_t time)
 {
-	_ekf->setMagData(magSample{time, _mag_data});
+	_ekf->setMagData(magSample{time, _mag_data + _bias});
 }
 
 void Mag::setData(const Vector3f &mag)

src/modules/ekf2/test/sensor_simulator/mag.h

@@ -52,9 +52,11 @@ public:
 	~Mag();
 
 	void setData(const Vector3f &mag);
+	void setBias(const Vector3f &bias) { _bias = bias; }
 
 private:
 	Vector3f _mag_data;
+	Vector3f _bias;
 
 	void send(uint64_t time) override;
 

src/modules/ekf2/test/test_EKF_mag.cpp

@@ -271,3 +271,41 @@ TEST_F(EkfMagTest, velocityRotationOnYawReset)
 	const float yaw_change = fabsf(wrap_pi(yaw_after - yaw_before));
 	EXPECT_GT(yaw_change, 0.3f) << "Yaw change: " << degrees(yaw_change) << " deg";
 }
+
+TEST_F(EkfMagTest, magFaultCleared)
+{
+	// GIVEN: biased mag data
+	_sensor_simulator._mag.setBias(Vector3f(-0.3f, 0.2f, 0.f));
+	_ekf_wrapper.enableGpsFusion();
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(11);
+
+	// THEN: the initial heading is incorrect
+	EXPECT_NEAR(degrees(_ekf_wrapper.getYawAngle()), -110.f, 5.f);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+
+	// WHEN: motion allows the yaw estimator to converge
+	_sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, -1.f));
+	_ekf->set_in_air_status(true);
+
+	_sensor_simulator.runTrajectorySeconds(3.f);
+
+	// THEN: the heading error is detected, solved and mag is declared faulty
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+	EXPECT_TRUE(_ekf_wrapper.isMagFaultDetected());
+
+	// BUT when: the mag disturbance is gone
+	_sensor_simulator._mag.setBias(Vector3f());
+	_sensor_simulator.setTrajectoryTargetVelocity(Vector3f(0.f, 0.f, 0.f));
+
+	_sensor_simulator.runTrajectorySeconds(7.f);
+
+	// THEN: the fault is cleared and mag fusion restarts
+	EXPECT_FALSE(_ekf_wrapper.isMagFaultDetected());
+	EXPECT_TRUE(_ekf_wrapper.isMagHeadingConsistent());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); // because in-flight alignment is required
+}