From d3da4fe608fa3d81e8eb315890d0be964eaea064 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 14 Jan 2026 14:13:28 +0100 Subject: [PATCH] ekf2 mag: clear mag_fault when healthy again --- .../aid_sources/magnetometer/mag_control.cpp | 28 ++++++++------ src/modules/ekf2/EKF/ekf_helper.cpp | 13 +++++-- .../test/sensor_simulator/ekf_wrapper.cpp | 10 +++++ .../ekf2/test/sensor_simulator/ekf_wrapper.h | 2 + .../ekf2/test/sensor_simulator/mag.cpp | 2 +- src/modules/ekf2/test/sensor_simulator/mag.h | 2 + src/modules/ekf2/test/test_EKF_mag.cpp | 38 +++++++++++++++++++ 7 files changed, 79 insertions(+), 16 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 6fdb679c8b..b700d9eb07 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -168,7 +168,13 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) checkMagHeadingConsistency(mag_sample); + if (_control_status.flags.mag_fault && _control_status.flags.mag_heading_consistent + && isTimedOut(_time_last_heading_fuse, _params.reset_timeout_max)) { + _control_status.flags.mag_fault = false; + } + { + const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !isNorthEastAidingActive(); const bool common_conditions_passing = _control_status.flags.mag && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) @@ -188,8 +194,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) || (_params.ekf2_mag_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); } - // TODO: allow clearing mag_fault if mag_3d is good? - if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { ECL_INFO("starting mag 3D fusion"); @@ -206,10 +210,18 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) if (continuing_conditions_passing && _control_status.flags.yaw_align) { - if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D - || _control_status.flags.yaw_manual)) - || (wmm_updated && no_ne_aiding_or_not_moving)) { + if (checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D + || _control_status.flags.yaw_manual)) { ECL_INFO("reset to %s", AID_SRC_NAME); + const bool reset_heading = ((_control_status.flags.mag_hdg || _control_status.flags.mag_3D) && !isNorthEastAidingActive()); + resetMagStates(_mag_lpf.getState(), reset_heading); + + // record the start time for the magnetic field alignment + _control_status.flags.mag_aligned_in_flight = true; + _flt_mag_align_start_time = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; + + } else if (wmm_updated && no_ne_aiding_or_not_moving) { const bool reset_heading = _control_status.flags.mag_hdg || _control_status.flags.mag_3D; resetMagStates(_mag_lpf.getState(), reset_heading); aid_src.time_last_fuse = imu_sample.time_us; @@ -443,12 +455,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) (double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2), (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)); } - - // record the start time for the magnetic field alignment - if (_control_status.flags.in_air && (reset_heading || _control_status.flags.yaw_manual)) { - _control_status.flags.mag_aligned_in_flight = true; - _flt_mag_align_start_time = _time_delayed_us; - } } void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5820494f8e..d568487da6 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1263,10 +1263,15 @@ void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const float Ekf::getHeadingInnov() const { + float innov = 0.f; + #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - return Vector3f(_aid_src_mag.innovation).max(); + innov = Vector3f(_aid_src_mag.innovation).max(); + + } else { + innov = _mag_heading_innov_lpf.getState(); } #endif // CONFIG_EKF2_MAGNETOMETER @@ -1274,7 +1279,7 @@ float Ekf::getHeadingInnov() const #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gnss_yaw) { - return _aid_src_gnss_yaw.innovation; + innov = _aid_src_gnss_yaw.innovation; } #endif // CONFIG_EKF2_GNSS_YAW @@ -1282,12 +1287,12 @@ float Ekf::getHeadingInnov() const #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - return _aid_src_ev_yaw.innovation; + innov = _aid_src_ev_yaw.innovation; } #endif // CONFIG_EKF2_EXTERNAL_VISION - return 0.f; + return innov; } float Ekf::getHeadingInnovVar() const diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 2d9a88f3ae..635b9b2704 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -210,6 +210,11 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const return _ekf->control_status_flags().ev_yaw; } +bool EkfWrapper::isIntendingMagFusion() const +{ + return _ekf->control_status_flags().mag; +} + bool EkfWrapper::isIntendingMagHeadingFusion() const { return _ekf->control_status_flags().mag_hdg; @@ -225,6 +230,11 @@ bool EkfWrapper::isMagHeadingConsistent() const return _ekf->control_status_flags().mag_heading_consistent; } +bool EkfWrapper::isMagFaultDetected() const +{ + return _ekf->control_status_flags().mag_fault; +} + void EkfWrapper::setMagFuseTypeNone() { _ekf_params->ekf2_mag_type = MagFuseType::NONE; diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 6601670eaf..4e36c19676 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -102,9 +102,11 @@ public: void disableExternalVisionHeadingFusion(); bool isIntendingExternalVisionHeadingFusion() const; + bool isIntendingMagFusion() const; bool isIntendingMagHeadingFusion() const; bool isIntendingMag3DFusion() const; bool isMagHeadingConsistent() const; + bool isMagFaultDetected() const; void setMagFuseTypeNone(); void enableMagStrengthCheck(); void enableMagInclinationCheck(); diff --git a/src/modules/ekf2/test/sensor_simulator/mag.cpp b/src/modules/ekf2/test/sensor_simulator/mag.cpp index 6a407b9c65..d5151a3d31 100644 --- a/src/modules/ekf2/test/sensor_simulator/mag.cpp +++ b/src/modules/ekf2/test/sensor_simulator/mag.cpp @@ -15,7 +15,7 @@ Mag::~Mag() void Mag::send(uint64_t time) { - _ekf->setMagData(magSample{time, _mag_data}); + _ekf->setMagData(magSample{time, _mag_data + _bias}); } void Mag::setData(const Vector3f &mag) diff --git a/src/modules/ekf2/test/sensor_simulator/mag.h b/src/modules/ekf2/test/sensor_simulator/mag.h index 202ba63fb9..04808d20fd 100644 --- a/src/modules/ekf2/test/sensor_simulator/mag.h +++ b/src/modules/ekf2/test/sensor_simulator/mag.h @@ -52,9 +52,11 @@ public: ~Mag(); void setData(const Vector3f &mag); + void setBias(const Vector3f &bias) { _bias = bias; } private: Vector3f _mag_data; + Vector3f _bias; void send(uint64_t time) override; diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index 32fccea707..3d3c43d111 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -271,3 +271,41 @@ TEST_F(EkfMagTest, velocityRotationOnYawReset) const float yaw_change = fabsf(wrap_pi(yaw_after - yaw_before)); EXPECT_GT(yaw_change, 0.3f) << "Yaw change: " << degrees(yaw_change) << " deg"; } + +TEST_F(EkfMagTest, magFaultCleared) +{ + // GIVEN: biased mag data + _sensor_simulator._mag.setBias(Vector3f(-0.3f, 0.2f, 0.f)); + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(11); + + // THEN: the initial heading is incorrect + EXPECT_NEAR(degrees(_ekf_wrapper.getYawAngle()), -110.f, 5.f); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + + // WHEN: motion allows the yaw estimator to converge + _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, -1.f)); + _ekf->set_in_air_status(true); + + _sensor_simulator.runTrajectorySeconds(3.f); + + // THEN: the heading error is detected, solved and mag is declared faulty + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + EXPECT_TRUE(_ekf_wrapper.isMagFaultDetected()); + + // BUT when: the mag disturbance is gone + _sensor_simulator._mag.setBias(Vector3f()); + _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(0.f, 0.f, 0.f)); + + _sensor_simulator.runTrajectorySeconds(7.f); + + // THEN: the fault is cleared and mag fusion restarts + EXPECT_FALSE(_ekf_wrapper.isMagFaultDetected()); + EXPECT_TRUE(_ekf_wrapper.isMagHeadingConsistent()); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); // because in-flight alignment is required +}