From 2bafb76334b63314b09b24107cb3b78887dbaf73 Mon Sep 17 00:00:00 2001 From: Silvan Date: Wed, 12 Nov 2025 18:13:26 +0100 Subject: [PATCH] EKF2: gps_yaw_reset_timeout_max Signed-off-by: Silvan --- src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp | 4 ++-- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/test/test_EKF_gnss_yaw.cpp | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index a0d2c4cb66..e8c6d1f8ce 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -75,7 +75,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) fuseGnssYaw(gnss_sample.yaw_offset); - const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); + const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_gps_yaw_max); if (is_fusion_failing) { stopGnssYawFusion(); @@ -123,7 +123,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) } } else if (_control_status.flags.gnss_yaw - && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) { + && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_gps_yaw_max)) { // No yaw data in the message anymore. Stop until it comes back. stopGnssYawFusion(); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index c8d61754aa..662de8f3c9 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -480,6 +480,7 @@ struct parameters { const unsigned reset_timeout_max{7'000'000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec) const unsigned no_aid_timeout_max{1'000'000}; ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec) const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec) + const unsigned reset_timeout_gps_yaw_max{3'000'000}; ///< maximum time we allow without GPS yaw int32_t ekf2_noaid_tout{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) diff --git a/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp index 93dd222375..0f9cb7d7d0 100644 --- a/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp @@ -273,7 +273,7 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround) const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f)); _sensor_simulator._gps.setYaw(gps_heading); - _sensor_simulator.runSeconds(8); + _sensor_simulator.runSeconds(4); // THEN: the fusion should stop, reset to mag EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());