mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
AP_GPS: Fix bug in calculation of blended GPS delay
This commit is contained in:
parent
87abccef6c
commit
aafacd0f8e
@ -1450,7 +1450,7 @@ void AP_GPS::calc_blended_state(void)
|
||||
if (_blend_weights[i] > 0.0f) {
|
||||
temp_time_1 += (double)timing[i].last_fix_time_ms * (double) _blend_weights[i];
|
||||
temp_time_2 += (double)timing[i].last_message_time_ms * (double)_blend_weights[i];
|
||||
_blended_lag_sec += get_lag(i) * _blended_lag_sec;
|
||||
_blended_lag_sec += get_lag(i) * _blend_weights[i];
|
||||
}
|
||||
}
|
||||
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1;
|
||||
|
Loading…
Reference in New Issue
Block a user