From 65a587e56a84f1b28f5a19c6081882599ee74921 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Mar 2022 21:17:49 -0400 Subject: [PATCH] ekf2: mag fusion don't update all states if mag_fault or mag_field_disturbed --- src/modules/ekf2/EKF/mag_control.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index f0183b7de2..e9c3461484 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -353,7 +353,8 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) { // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise // before they are used to constrain heading drift - const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); + const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6) + && !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed; if (!_mag_decl_cov_reset) { // After any magnetic field covariance reset event the earth field state