fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 mag: clear mag_fault when healthy again

Browse Source
This commit is contained in:
bresch 2026-01-14 14:13:28 +01:00 committed by Mathieu Bresciani
parent 76b58b6f0b
commit d3da4fe608
7 changed files with 79 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -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)

13
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -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

10
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
View File

@ -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;

2
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
View File

@ -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();

2
src/modules/ekf2/test/sensor_simulator/mag.cpp
View File

@ -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)

2
src/modules/ekf2/test/sensor_simulator/mag.h
View File

@ -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;

38
src/modules/ekf2/test/test_EKF_mag.cpp
View File

@ -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
}