ekf2-mag: rework mag strength check

src/modules/ekf2/EKF/common.h

@@ -155,6 +155,12 @@ enum class EvCtrl : uint8_t {
 	YAW  = (1<<3)
 };
 
+enum class MagCheckMask : uint8_t {
+	STRENGTH    = (1 << 0),
+	INCLINATION = (1 << 1),
+	FORCE_WMM   = (1 << 2)
+};
+
 struct gpsMessage {
 	uint64_t    time_usec{};
 	int32_t     lat{};              ///< Latitude in 1E-7 degrees
@@ -486,7 +492,8 @@ struct parameters {
 
 	// compute synthetic magnetomter Z value if possible
 	int32_t synthesize_mag_z{0};
-	int32_t check_mag_strength{0};
+	int32_t mag_check{0};
+	float mag_check_strength_tolerance_gs{0.2f};
 
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
 	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)

src/modules/ekf2/EKF/ekf.h

@@ -585,6 +585,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
+	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
 
 	SquareMatrix24f P{};	///< state covariance matrix
 
@@ -707,6 +708,7 @@ private:
 	// Variables used to control activation of post takeoff functionality
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
+	uint64_t _time_last_mag_check_failing{0};
 	Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
 	Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
 	float _saved_mag_ef_d_variance{};   ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
@@ -1035,7 +1037,7 @@ private:
 
 	void checkMagDeclRequired();
 	bool shouldInhibitMag() const;
-	bool magFieldStrengthDisturbed(const Vector3f &mag) const;
+	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
 	void runMagAndMagDeclFusions(const Vector3f &mag);
 	void run3DMagAndDeclFusions(const Vector3f &mag);

src/modules/ekf2/EKF/mag_control.cpp

@@ -42,6 +42,7 @@
 void Ekf::controlMagFusion()
 {
 	bool mag_data_ready = false;
+	bool mag_data_valid = false;
 
 	magSample mag_sample;
 
@@ -85,8 +86,7 @@ void Ekf::controlMagFusion()
 				_control_status.flags.synthetic_mag_z = false;
 			}
 
-			_control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag);
-
+			mag_data_valid = checkMagField(mag_sample.mag);
 
 			// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
 			const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
@@ -120,7 +120,7 @@ void Ekf::controlMagFusion()
 		_control_status.flags.mag_aligned_in_flight = false;
 	}
 
-	if (mag_data_ready && !_control_status.flags.tilt_align && !_control_status.flags.yaw_align) {
+	if (mag_data_ready && mag_data_valid && !_control_status.flags.tilt_align && !_control_status.flags.yaw_align) {
 		// calculate the initial magnetic field and yaw alignment
 		// but do not mark the yaw alignement complete as it needs to be
 		// reset once the leveling phase is done
@@ -169,7 +169,7 @@ void Ekf::controlMagFusion()
 
 	if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
 
-		if (shouldInhibitMag()) {
+		if (shouldInhibitMag() || !mag_data_valid) {
 			if (uint32_t(_time_delayed_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
 				// If magnetometer use has been inhibited continuously then stop the fusion
 				stopMagFusion();
@@ -432,23 +432,50 @@ bool Ekf::shouldInhibitMag() const
 	return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
 }
 
-bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const
+bool Ekf::checkMagField(const Vector3f &mag_sample)
 {
-	if (_params.check_mag_strength
-	    && ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
+	_control_status.flags.mag_field_disturbed = false;
 
+	if ((_params.mag_fusion_type == MagFuseType::NONE)
+	    || (_params.mag_fusion_type == MagFuseType::INDOOR && !_control_status.flags.gps)) {
+		//TODO: review this: gps flag cannnot be set if yaw isn't aligned
+		return false;
+	}
+
+	if (_params.mag_check == 0) {
+		// skip all checks
+		return true;
+	}
+
+	bool is_check_failing = false;
+	const float mag_strength = mag_sample.length();
+
+	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
 		if (PX4_ISFINITE(_mag_strength_gps)) {
-			constexpr float wmm_gate_size = 0.2f; // +/- Gauss
-			return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size);
+			if (!isMeasuredMatchingExpected(mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
+				_control_status.flags.mag_field_disturbed = true;
+				is_check_failing = true;
+			}
+
+		} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
+			is_check_failing = true;
 
 		} else {
 			constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
 			constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
-			return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
+
+			if (!isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size)) {
+				_control_status.flags.mag_field_disturbed = true;
+				is_check_failing = true;
+			}
 		}
 	}
 
-	return false;
+	if (is_check_failing || (_time_last_mag_check_failing == 0)) {
+		_time_last_mag_check_failing = _time_delayed_us;
+	}
+
+	return ((_time_delayed_us - _time_last_mag_check_failing) > (uint64_t)_min_mag_health_time_us);
 }
 
 bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
@@ -527,13 +554,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag)
 
 	const Vector3f mag_init = _mag_lpf.getState() - mag_bias;
 
-	const bool mag_available = !magFieldStrengthDisturbed(mag_init);
-
-	// low pass filtered mag required
-	if (!mag_available) {
-		return false;
-	}
-
 	const bool heading_required_for_navigation = _control_status.flags.gps;
 
 	if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {