From 1b1004daf2987ab5e0f99c5b27e85657e5ef3a77 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Sep 2023 09:43:30 -0400 Subject: [PATCH] ekf2: mag 3d control HAGL reset wait until observable --- src/modules/ekf2/EKF/mag_3d_control.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index b45621697f..df538ffaf5 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) && _control_status.flags.tilt_align && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6)) + && isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6) && mag_sample.mag.longerThan(0.f) && mag_sample.mag.isAllFinite(); @@ -165,6 +165,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star // activate fusion, reset mag states and initialize variance if first init or in flight reset if (!_control_status.flags.yaw_align || wmm_updated + || checkHaglYawResetReq() || !_mag_decl_cov_reset || !_state.mag_I.longerThan(0.f) || (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I