From eba0b999502f416f5e7fdbf0635dbaa1826f2c57 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 2 Sep 2025 13:14:20 +0200 Subject: [PATCH] ekf2: remove unnecessary code The delta angles are now correctly accumulated in case multiple resets are triggered during the same epoch --- src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp | 8 -------- .../ekf2/EKF/aid_sources/magnetometer/mag_control.cpp | 2 -- 2 files changed, 10 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index d265a0bc26..76936556a4 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -522,14 +522,6 @@ bool Ekf::resetYawToEKFGSF() return false; } - // don't allow reset if there's just been a yaw reset - const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); - const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); - - if (yaw_alignment_changed || quat_reset) { - return false; - } - ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); 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 991f7bc75f..d178bd8ebf 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -593,8 +593,6 @@ void Ekf::resetMagHeading(const Vector3f &mag) // update quaternion states and corresponding covarainces resetQuatStateYaw(yaw_new, yaw_new_variance); - _time_last_heading_fuse = _time_delayed_us; - _mag_heading_innov_lpf.reset(0.f); _control_status.flags.mag_heading_consistent = true; }