diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 1ea3915cff..f20fb37b20 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/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) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 7126f57c8b..52a1677576 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/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 { diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index efe551c865..47d2bbeaa7 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/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(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(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; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d25b358eff..a5c08e5d55 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -191,6 +191,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): #endif // CONFIG_EKF2_BARO_COMPENSATION _param_ekf2_mag_check(_params->mag_check), _param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs), + _param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg), _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) { @@ -2532,7 +2533,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) if (!_mag_decl_saved) { float declination_deg; - if (_ekf.get_mag_decl_deg(&declination_deg)) { + if (_ekf.get_mag_decl_deg(declination_deg)) { _param_ekf2_mag_decl.update(); if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 67e969cee1..63f7fff540 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -738,6 +738,7 @@ private: (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check (ParamExtFloat) _param_ekf2_mag_chk_str, + (ParamExtFloat) _param_ekf2_mag_chk_inc, (ParamExtInt) _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index b52fc0f6ef..a6f9f70922 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1510,7 +1510,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); * * Set bits to 1 to enable checks. Checks enabled by the following bit positions * 0 : Magnetic field strength. Set tolerance using EKF2_MAG_CHK_STR - * 1 : Reserved + * 1 : Magnetic field inclination. Set tolerance using EKF2_MAG_CHK_INC * 2 : Wait for GNSS to find the theoretical strength and inclination using the WMM * * @group EKF2 @@ -1518,7 +1518,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); * @min 0 * @max 7 * @bit 0 Strength (EKF2_MAG_CHK_STR) - * @bit 1 Reserved + * @bit 1 Inclination (EKF2_MAG_CHK_INC) * @bit 2 Wait for WMM */ PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1); @@ -1536,6 +1536,19 @@ PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1); */ PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_STR, 0.2f); +/** + * Magnetic field inclination check tolerance + * + * Maximum allowed deviation from the expected magnetic field inclination to pass the check. + * + * @group EKF2 + * @min 0.0 + * @max 90.0 + * @unit deg + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_INC, 20.f); + /** * Enable synthetic magnetometer Z component measurement. * diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 1df54d8c61..5693c99f96 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -214,6 +214,15 @@ void EkfWrapper::enableMagStrengthCheck() { _ekf_params->mag_check |= static_cast(MagCheckMask::STRENGTH); } + +void EkfWrapper::enableMagInclinationCheck() +{ + _ekf_params->mag_check |= static_cast(MagCheckMask::INCLINATION); +} + +void EkfWrapper::enableMagCheckForceWMM() +{ + _ekf_params->mag_check |= static_cast(MagCheckMask::FORCE_WMM); } bool EkfWrapper::isWindVelocityEstimated() const diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 73ea368166..6adfc5ce9a 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -102,6 +102,8 @@ public: bool isIntendingMag3DFusion() const; void setMagFuseTypeNone(); void enableMagStrengthCheck(); + void enableMagInclinationCheck(); + void enableMagCheckForceWMM(); bool isWindVelocityEstimated() const; diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index 9970f53cc6..ec55eeafb9 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -95,7 +95,7 @@ TEST_F(EkfMagTest, fusionStartWithReset) Vector3f mag_earth = _ekf->getMagEarthField(); float mag_decl = atan2f(mag_earth(1), mag_earth(0)); float mag_decl_wmm_deg = 0.f; - _ekf->get_mag_decl_deg(&mag_decl_wmm_deg); + _ekf->get_mag_decl_deg(mag_decl_wmm_deg); EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-6f); } @@ -141,3 +141,70 @@ TEST_F(EkfMagTest, suddenLargeStrength) EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); } + +TEST_F(EkfMagTest, noInitLargeInclination) +{ + // GIVEN: a really large magnetic field + _ekf_wrapper.enableMagInclinationCheck(); + // To prevent an early pass of the inclination check, "force WMM" must be set + _ekf_wrapper.enableMagCheckForceWMM(); + _sensor_simulator.startGps(); + Vector3f mag_data(0.4f, 0.f, 0.f); + _sensor_simulator._mag.setData(mag_data); + + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + _sensor_simulator.runSeconds(_init_duration_s + 10.f); // live some extra time fo GNSS checks to pass + + // THEN: the fusion shouldn't start + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); + + // BUT then: as soon as there is some meaningful data + const float mag_heading = -M_PI_F / 7.f; + mag_data = Vector3f(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + _sensor_simulator._mag.setData(mag_data); + + _sensor_simulator.runSeconds(2.f); + + float decl_deg = 0.f; + _ekf->get_mag_decl_deg(decl_deg); + + // THEN: the fusion initializes using the mag data and runs normally + EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f)); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); +} + +TEST_F(EkfMagTest, suddenInclinationChange) +{ + _ekf_wrapper.enableMagInclinationCheck(); + _ekf_wrapper.enableMagCheckForceWMM(); + _sensor_simulator.startGps(); + + // GIVEN: some meaningful mag data + const float mag_heading = -M_PI_F / 7.f; + Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + _sensor_simulator._mag.setData(mag_data); + + _sensor_simulator.runSeconds(_init_duration_s + 10.f); + + float decl_deg = 0.f; + _ekf->get_mag_decl_deg(decl_deg); + + // THEN: the fusion initializes using the mag data and runs normally + EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f)); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + + // BUT WHEN: the mag field inclination suddenly changes + mag_data(2) = -mag_data(2); + _sensor_simulator._mag.setData(mag_data); + _sensor_simulator.runSeconds(6.f); + + // THEN: the mag fusion should stop after some time + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); +}