Browse Source

AP_GPS: Fix bug in calculation of blended GPS delay

master
priseborough 8 years ago committed by Francisco Ferreira
parent
commit
bd0229b7bc
  1. 2
      libraries/AP_GPS/AP_GPS.cpp

2
libraries/AP_GPS/AP_GPS.cpp

@ -1454,7 +1454,7 @@ void AP_GPS::calc_blended_state(void) @@ -1454,7 +1454,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…
Cancel
Save