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 <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen 2021-05-02 20:59:50 +03:00 committed by GitHub
parent e78a4287f9
commit fa9fdce6e6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 2 deletions

View File

@ -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];