From f499749c2332d53c7240d332097eb4e7d2bb15ba Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 25 Jan 2022 14:24:22 -0500 Subject: [PATCH] ekf2: mag field strength check respect requirements - requires valid mag data (_mag_data_ready) - respect mag_fusion_type (including MAG_FUSE_TYPE_INDOOR) --- src/modules/ekf2/EKF/ekf.h | 2 -- src/modules/ekf2/EKF/mag_control.cpp | 37 ++++++++++++---------------- 2 files changed, 16 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index d416f9ef56..cfcfc4c792 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -852,8 +852,6 @@ private: bool shouldInhibitMag() const; void checkMagFieldStrength(); bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; } - bool isMeasuredMatchingGpsMagStrength() const; - bool isMeasuredMatchingAverageMagStrength() const; static bool isMeasuredMatchingExpected(float measured, float expected, float gate); void runMagAndMagDeclFusions(); void run3DMagAndDeclFusions(); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index da3703a0d0..6946c1ab4c 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -302,31 +302,26 @@ bool Ekf::shouldInhibitMag() const void Ekf::checkMagFieldStrength() { - if (_params.check_mag_strength) { - _control_status.flags.mag_field_disturbed = _NED_origin_initialised - ? !isMeasuredMatchingGpsMagStrength() - : !isMeasuredMatchingAverageMagStrength(); + if (_mag_data_ready) { + if (_params.check_mag_strength + && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) { + + if (PX4_ISFINITE(_mag_strength_gps)) { + constexpr float wmm_gate_size = 0.2f; // +/- Gauss + _control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size); + + } else { + constexpr float average_earth_mag_field_strength = 0.45f; // Gauss + constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss + _control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); + } - } else { - _control_status.flags.mag_field_disturbed = false; + } else { + _control_status.flags.mag_field_disturbed = false; + } } } -bool Ekf::isMeasuredMatchingGpsMagStrength() const -{ - constexpr float wmm_gate_size = 0.2f; // +/- Gauss - return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size); -} - -bool Ekf::isMeasuredMatchingAverageMagStrength() const -{ - constexpr float average_earth_mag_field_strength = 0.45f; // Gauss - constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss - return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), - average_earth_mag_field_strength, - average_earth_mag_gate_size); -} - bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate) { return (measured >= expected - gate)