From 9efadad06a9c9e4f383a29c8062f45c968af2d13 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Mar 2022 14:27:27 -0400 Subject: [PATCH] ekf2: move checkMagFieldStrength() to magFieldStrengthDisturbed() const method --- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/mag_control.cpp | 17 +++++++---------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e7d769e29f..59fe2faa93 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -863,7 +863,7 @@ private: void checkMagDeclRequired(); void checkMagInhibition(); bool shouldInhibitMag() const; - void checkMagFieldStrength(const Vector3f &mag); + bool magFieldStrengthDisturbed(const Vector3f &mag) const; 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 90b994fc90..f0183b7de2 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -64,11 +64,9 @@ void Ekf::controlMagFusion() } else { _control_status.flags.synthetic_mag_z = false; } - } - } - if (mag_data_ready) { - checkMagFieldStrength(mag_sample.mag); + _control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag); + } } // If we are on ground, reset the flight alignment flag so that the mag fields will be @@ -302,24 +300,23 @@ bool Ekf::shouldInhibitMag() const return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed; } -void Ekf::checkMagFieldStrength(const Vector3f &mag_sample) +bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const { if (_params.check_mag_strength && ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::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.length(), _mag_strength_gps, wmm_gate_size); + return !isMeasuredMatchingExpected(mag_sample.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.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); + return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); } - - } else { - _control_status.flags.mag_field_disturbed = false; } + + return false; } bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)