From 36837f03970d8f0007f1ddec26c526d1c9f622af Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 15 Jun 2018 08:05:16 +1000 Subject: [PATCH] EKF: Prevent timestamp jitter failing GPS data arrival check 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"); }