From b5217412f947db14d3018eeba0c3a7e1917bfc4c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Sep 2019 21:14:41 +1000 Subject: [PATCH] AP_GPS: fixed a bug in handling corrupt u-blox packets when we have corrupt input due to loss of bytes on a UART we can end up with a u-blox packet with zero payload bytes. When that happens we need to bypass payload reading as otherwise we will end up keeping reading bytes until the driver resets at 4 seconds, causing a GPS outage. This was causing GPS outages every few hours in copters running 3.6.7, and was also reproduced in SITL using SIM_GPS_BYTELOSS. --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 2cb7daffea..74cce72ad0 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -467,6 +467,10 @@ AP_GPS_UBLOX::read(void) goto reset; } _payload_counter = 0; // prepare to receive payload + if (_payload_length == 0) { + // bypass payload and go straight to checksum + _step++; + } break; // Receive message data