From 1a1106878d6ab1cc48565bef9b25b07d626c5d0b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 15 Jun 2018 16:03:54 +1000 Subject: [PATCH] EKF: Prevent timestamp jitter failing GPS data arrival check (#462) This fixes a error condition that occurs if _time_last_gps is greater than _time_last_imu. By checking time stamps at the fusion time horizon, we guarantee that this cannot happen because all observations must have a time stamp smaller or equal to _imu_sample_delayed.time_us before they are retrieved from the buffers. --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 46de191a3a..f704168283 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -612,7 +612,7 @@ void Ekf::controlGpsFusion() _hvelInnovGate = fmaxf(_params.vel_innov_gate, 1.0f); } - } else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) { + } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { _control_status.flags.gps = false; ECL_WARN("EKF GPS data stopped"); }