From e4a16bfc800b5fbd380d4dafd0d7b500f1c3b3c3 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 14 Jul 2023 17:28:19 +0200 Subject: [PATCH] ekf2-mag: rework mag strength check --- src/modules/ekf2/EKF/common.h | 9 ++- src/modules/ekf2/EKF/ekf.h | 4 +- src/modules/ekf2/EKF/mag_control.cpp | 56 +++++++++++++------ src/modules/ekf2/EKF2.cpp | 3 +- src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/ekf2_params.c | 29 +++++++++- .../test/sensor_simulator/ekf_wrapper.cpp | 3 +- 7 files changed, 80 insertions(+), 25 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 5610885d14..1ea3915cff 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/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) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e4aad32fb5..242684bb70 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 551fa4ddd1..efe551c865 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/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(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(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)) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f5180c59a1..d25b358eff 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -189,7 +189,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), _param_ekf2_pcoef_z(_params->static_pressure_coef_z), #endif // CONFIG_EKF2_BARO_COMPENSATION - _param_ekf2_mag_check(_params->check_mag_strength), + _param_ekf2_mag_check(_params->mag_check), + _param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs), _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index c3da913716..67e969cee1 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -737,6 +737,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, (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 63ecba71c1..b52fc0f6ef 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1502,17 +1502,40 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); /** * Magnetic field strength test selection * - * When set, the EKF checks the strength of the magnetic field - * to decide whether the magnetometer data is valid. - * If GPS data is received, the magnetic field is compared to a World + * Bitmask to set which check is used to decide whether the magnetometer data is valid. + * + * If GNSS data is received, the magnetic field is compared to a World * Magnetic Model (WMM), otherwise an average value is used. * This check is useful to reject occasional hard iron disturbance. * + * 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 + * 2 : Wait for GNSS to find the theoretical strength and inclination using the WMM + * * @group EKF2 * @boolean + * @min 0 + * @max 7 + * @bit 0 Strength (EKF2_MAG_CHK_STR) + * @bit 1 Reserved + * @bit 2 Wait for WMM */ PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1); +/** + * Magnetic field strength check tolerance + * + * Maximum allowed deviation from the expected magnetic field strength to pass the check. + * + * @group EKF2 + * @min 0.0 + * @max 1.0 + * @unit gauss + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_STR, 0.2f); + /** * 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 adf31b1c1c..1df54d8c61 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -212,7 +212,8 @@ void EkfWrapper::setMagFuseTypeNone() void EkfWrapper::enableMagStrengthCheck() { - _ekf_params->check_mag_strength = 1; + _ekf_params->mag_check |= static_cast(MagCheckMask::STRENGTH); +} } bool EkfWrapper::isWindVelocityEstimated() const