Browse Source

AP_GPS: don't accept infinite accuracies for blending

these result in NaN values for velocities
zr-v5.1
Andrew Tridgell 4 years ago
parent
commit
05194ed8b6
  1. 5
      libraries/AP_GPS/AP_GPS.cpp

5
libraries/AP_GPS/AP_GPS.cpp

@ -1497,6 +1497,11 @@ bool AP_GPS::calc_blend_weights(void) @@ -1497,6 +1497,11 @@ bool AP_GPS::calc_blend_weights(void)
if (get_rate_ms(i) > max_rate_ms) {
max_rate_ms = get_rate_ms(i);
}
if (isinf(state[i].speed_accuracy) ||
isinf(state[i].horizontal_accuracy) ||
isinf(state[i].vertical_accuracy)) {
return false;
}
}
if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) {
// data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated

Loading…
Cancel
Save