From 34a911fa184b439e4e0438527ddc71e00e3a8067 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 25 Apr 2022 10:26:49 -0400 Subject: [PATCH] ekf2: increase yaw estimator heading fusion rate --- src/modules/ekf2/EKF/gps_control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index e689235da8..f03244e575 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -68,8 +68,8 @@ void Ekf::controlGpsFusion() // periodically fuse yaw estimate if necessary if (isYawEmergencyEstimateAvailable() && _control_status.flags.yaw_align - && isTimedOut(_ekfgsf_yaw_reset_time, 1'000'000) - && isTimedOut(_time_last_heading_fuse, (uint64_t)500'000)) { + && isTimedOut(_ekfgsf_yaw_reset_time, 500'000) + && isTimedOut(_time_last_heading_fuse, (uint64_t)300'000)) { float innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yawEstimator.getYaw()); float obs_var = _yawEstimator.getYawVar();