From 422944ce295b5907384118871d94a30883d876e5 Mon Sep 17 00:00:00 2001
From: Werner Backes <op-dev@bit-1.de>
Date: Mon, 9 Apr 2012 23:12:24 +0200
Subject: [PATCH] Experimental extension to the GPS stream parser to extract
 u-blox binary data format. See "u-blox 6 Receiver Description" for reference.
 GPS data stream may carry mixed NMEA and UBX data.

---
 flight/Modules/GPS/GPS.c | 103 ++++++++++++++++++++++++++++++++++-----
 1 file changed, 92 insertions(+), 11 deletions(-)

diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c
index 716e47aa3..a9f9c6d97 100644
--- a/flight/Modules/GPS/GPS.c
+++ b/flight/Modules/GPS/GPS.c
@@ -36,11 +36,14 @@
 #include <stdbool.h>
 
 #include "NMEA.h"
+#include "UBX.h"
+
 
 #include "gpsposition.h"
 #include "homelocation.h"
 #include "gpstime.h"
 #include "gpssatellites.h"
+#include "gpsvelocity.h"
 #include "WorldMagModel.h"
 #include "CoordinateConversions.h"
 #include "hwsettings.h"
@@ -137,6 +140,7 @@ int32_t GPSInitialize(void)
 
 	if (gpsPort && gpsEnabled) {
 		GPSPositionInitialize();
+		GPSVelocityInitialize();
 #if !defined(PIOS_GPS_MINIMAL)
 		GPSTimeInitialize();
 		GPSSatellitesInitialize();
@@ -167,10 +171,14 @@ static void gpsTask(void *parameters)
 	portTickType xDelay = 100 / portTICK_RATE_MS;
 	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
 	GPSPositionData GpsData;
+	UBXPacket *ubx = (UBXPacket *)gps_rx_buffer;
 	
 	uint8_t rx_count = 0;
-	bool start_flag = false;
+//	bool start_flag = false;
 	bool found_cr = false;
+	enum proto_states {START,NMEA,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1,
+		UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2};
+	enum proto_states proto_state = START;
 	int32_t gpsRxOverflow = 0;
 	
 	numUpdates = 0;
@@ -184,6 +192,7 @@ static void gpsTask(void *parameters)
 	while (1)
 	{
 		uint8_t c;
+
 		// NMEA or SINGLE-SENTENCE GPS mode
 
 		// This blocks the task until there is something on the buffer
@@ -191,22 +200,94 @@ static void gpsTask(void *parameters)
 		{
 		
 			// detect start while acquiring stream
-			if (!start_flag && (c == '$'))
+			switch (proto_state)
 			{
-				start_flag = true;
-				found_cr = false;
-				rx_count = 0;
+				case START: // detect protocol
+					switch (c)
+					{
+						case UBX_SYNC1: // first UBX sync char found
+							proto_state = UBX_SY2;
+							continue;
+						case '$': // NMEA identifier found
+							proto_state = NMEA;
+							found_cr = false;
+							rx_count = 0;
+							break;
+						default:
+							continue;
+					}
+					break;
+				case UBX_SY2:
+					if (c == UBX_SYNC2) // second UBX sync char found
+					{
+						proto_state = UBX_CLASS;
+						found_cr = false;
+						rx_count = 0;
+					}
+					else
+					{
+						proto_state = START; // reset state
+					}
+					continue;
+				case UBX_CLASS:
+					ubx->header.class = c;
+					proto_state = UBX_ID;
+					continue;
+				case UBX_ID:
+					ubx->header.id = c;
+					proto_state = UBX_LEN1;
+					continue;
+				case UBX_LEN1:
+					ubx->header.len = c;
+					proto_state = UBX_LEN2;
+					continue;
+				case UBX_LEN2:
+					ubx->header.len += (c << 8);
+					if ((sizeof (UBXHeader)) + ubx->header.len > NMEA_MAX_PACKET_LENGTH)
+					{
+						gpsRxOverflow++;
+						proto_state = START;
+						found_cr = false;
+						rx_count = 0;
+					}
+					else
+					{
+						proto_state = UBX_PAYLOAD;
+					}
+					continue;
+				case UBX_PAYLOAD:
+					if (rx_count < ubx->header.len)
+					{
+						ubx->payload.payload[rx_count] = c;
+						if (++rx_count == ubx->header.len)
+							proto_state = UBX_CHK1;
+					}
+					else
+						proto_state = START;
+					continue;
+				case UBX_CHK1:
+					ubx->header.ck_a = c;
+					proto_state = UBX_CHK2;
+					continue;
+				case UBX_CHK2:
+					ubx->header.ck_b = c;
+					if (checksum_ubx_message(ubx))
+					{
+						parse_ubx_message(ubx);
+					}
+					proto_state = START;
+					continue;
+				case NMEA:
+					break;
 			}
-			else
-			if (!start_flag)
-				continue;
-		
+
+
 			if (rx_count >= NMEA_MAX_PACKET_LENGTH)
 			{
 				// The buffer is already full and we haven't found a valid NMEA sentence.
 				// Flush the buffer and note the overflow event.
 				gpsRxOverflow++;
-				start_flag = false;
+				proto_state = START;
 				found_cr = false;
 				rx_count = 0;
 			}
@@ -230,7 +311,7 @@ static void gpsTask(void *parameters)
 				gps_rx_buffer[rx_count-2] = 0;
 
 				// prepare to parse next sentence
-				start_flag = false;
+				proto_state = START;
 				found_cr = false;
 				rx_count = 0;
 				// Our rxBuffer must look like this now: