From fa9fdce6e6f26a1fbf5970aeb5eb7be3cf4941d3 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Sun, 2 May 2021 20:59:50 +0300 Subject: [PATCH] sensors/vehicle_gps_position: Fix raw_dt and present_dt calculation in gps_blending If the GPS data is passed from companion computer, there may be inaccuracies with timesync. This may cause time deltas to overflow. Make the time delta calculation using floats directly, wich results in negative numbers in case there are such inaccuracies. Signed-off-by: Jukka Laitinen --- src/modules/sensors/vehicle_gps_position/gps_blending.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 03d7f3a5cf..72ab461c9e 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -105,8 +105,8 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) _np_gps_suitable_for_blending = 0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - const float raw_dt = 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]); - const float present_dt = 1e-6f * (float)(hrt_now_us - _gps_state[i].timestamp); + const float raw_dt = 1e-6f * ((float)_gps_state[i].timestamp - (float)_time_prev_us[i]); + const float present_dt = 1e-6f * ((float)hrt_now_us - (float)_gps_state[i].timestamp); if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) { _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];