From 0dafabf55265ffc9e5b6e96e8b92d41ee4bd731e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Nov 2020 09:33:20 +1100 Subject: [PATCH] AP_GPS: don't accept infinite accuracies for blending these result in NaN values for velocities --- libraries/AP_GPS/AP_GPS.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 9ba2eac4b4..1f983b745e 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1415,6 +1415,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