From d359cdff3f0d6659398f92664e7f87c18dd3c983 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Nov 2020 09:32:25 +1100 Subject: [PATCH] AP_GPS: prevent UAVCAN GPS from giving infinite accuracy values this can happen due to the complex encodings of accuracies in UAVCAN --- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 433d8da0b9..d1d5c18e28 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -427,6 +427,12 @@ bool AP_GPS_UAVCAN::read(void) if (_new_data) { _new_data = false; + // the encoding of accuracies in UAVCAN can result in infinite + // values. These cause problems with blending. Use 1000m and 1000m/s instead + interim_state.horizontal_accuracy = MIN(interim_state.horizontal_accuracy, 1000.0); + interim_state.vertical_accuracy = MIN(interim_state.vertical_accuracy, 1000.0); + interim_state.speed_accuracy = MIN(interim_state.speed_accuracy, 1000.0); + state = interim_state; return true;