From 6b8bf8042314f21ab8ee153e1d4dced562299576 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 14 Apr 2025 14:29:25 +0200 Subject: [PATCH] ekf2-mag: never skip post-takeoff mag yaw reset --- .../ekf2/EKF/aid_sources/magnetometer/mag_control.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 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 927a83e24f..3b68644a1f 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -210,9 +210,11 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) if (continuing_conditions_passing && _control_status.flags.yaw_align) { - if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) { + if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D)) + || (wmm_updated && no_ne_aiding_or_not_moving)) { ECL_INFO("reset to %s", AID_SRC_NAME); - resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); + 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; } else { @@ -446,7 +448,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) } // record the start time for the magnetic field alignment - if (_control_status.flags.in_air) { + if (_control_status.flags.in_air && reset_heading) { _control_status.flags.mag_aligned_in_flight = true; _flt_mag_align_start_time = _time_delayed_us; }