From f7e5f881998403d067fd1bbd893c86b2dbfcb4dc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 5 Jan 2013 16:32:42 +1100
Subject: [PATCH] AP_GPS: make UBLOX driver a bit more robust

---
 libraries/AP_GPS/AP_GPS_UBLOX.cpp |  9 ++++++++-
 libraries/AP_GPS/AP_GPS_UBLOX.h   | 19 +++++++++----------
 2 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index abd67d0e34..6ee9c23b7b 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -73,6 +73,7 @@ AP_GPS_UBLOX::read(void)
         // read the next byte
         data = _port->read();
 
+	reset:
         switch(_step) {
 
         // Message preamble detection
@@ -131,6 +132,7 @@ AP_GPS_UBLOX::read(void)
                 // assume very large payloads are line noise
                 _payload_length = 0;
                 _step = 0;
+				goto reset;
             }
             _payload_counter = 0;                               // prepare to receive payload
             break;
@@ -152,7 +154,8 @@ AP_GPS_UBLOX::read(void)
             _step++;
             if (_ck_a != data) {
                 Debug("bad cka %x should be %x", data, _ck_a);
-                _step = 0;                                              // bad checksum
+                _step = 0;
+				goto reset;
             }
             break;
         case 8:
@@ -378,6 +381,7 @@ AP_GPS_UBLOX::_detect(uint8_t data)
 	static uint8_t step;
 	static uint8_t ck_a, ck_b;
 
+reset:
 	switch (step) {
         case 1:
             if (PREAMBLE2 == data) {
@@ -416,6 +420,7 @@ AP_GPS_UBLOX::_detect(uint8_t data)
             step++;
             if (ck_a != data) {
                 step = 0;
+				goto reset;
             }
             break;
         case 8:
@@ -423,6 +428,8 @@ AP_GPS_UBLOX::_detect(uint8_t data)
 			if (ck_b == data) {
 				// a valid UBlox packet
 				return true;
+			} else {
+				goto reset;
 			}
     }
     return false;
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index ed2477898e..2b5676e1d5 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -124,6 +124,15 @@ private:
         uint32_t speed_accuracy;
         uint32_t heading_accuracy;
     };
+    // Receive buffer
+    union {
+        ubx_nav_posllh posllh;
+        ubx_nav_status status;
+        ubx_nav_solution solution;
+        ubx_nav_velned velned;
+        ubx_cfg_nav_settings nav_settings;
+        uint8_t bytes[];
+    } _buffer;
 	#pragma pack(pop)
 
     enum ubs_protocol_bytes {
@@ -179,16 +188,6 @@ private:
 
     uint8_t         _disable_counter;
 
-    // Receive buffer
-    union {
-        ubx_nav_posllh posllh;
-        ubx_nav_status status;
-        ubx_nav_solution solution;
-        ubx_nav_velned velned;
-        ubx_cfg_nav_settings nav_settings;
-        uint8_t bytes[];
-    } _buffer;
-
     // Buffer parse & GPS state update
     bool        _parse_gps();