ekf2-mag: add inclination check

src/modules/ekf2/EKF/common.h

@@ -494,6 +494,7 @@ struct parameters {
 	int32_t synthesize_mag_z{0};
 	int32_t mag_check{0};
 	float mag_check_strength_tolerance_gs{0.2f};
+	float mag_check_inclination_tolerance_deg{20.f};
 
 	// 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/estimator_interface.h

@@ -216,10 +216,10 @@ public:
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
 	// At the next startup, set param.mag_declination_deg to the value saved
-	bool get_mag_decl_deg(float *val) const
+	bool get_mag_decl_deg(float &val) const
 	{
 		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
-			*val = math::degrees(_mag_declination_gps);
+			val = math::degrees(_mag_declination_gps);
 			return true;
 
 		} else {

src/modules/ekf2/EKF/mag_control.cpp

@@ -471,6 +471,28 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 		}
 	}
 
+	const Vector3f mag_earth = _R_to_earth * mag_sample;
+	const float mag_inclination = asin(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
+
+	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
+		if (PX4_ISFINITE(_mag_inclination_gps)) {
+			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
+			const float inc_error_rad = wrap_pi(mag_inclination - _mag_inclination_gps);
+
+			if (fabsf(inc_error_rad) > inc_tol_rad) {
+				_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 {
+			// No check possible when the global position is unknown
+			// TODO: add parameter to remember the inclination between boots
+		}
+	}
+
 	if (is_check_failing || (_time_last_mag_check_failing == 0)) {
 		_time_last_mag_check_failing = _time_delayed_us;
 	}