Merge branch 'D-Lite/ubx-parser' into revolution_ubx

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;
+				case START: // detect protocol
-				found_cr = false;
+					switch (c)
-				rx_count = 0;
+					{
+						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:

flight/Modules/GPS/UBX.c

@@ -0,0 +1,97 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup GSPModule GPS Module
+ * @brief Process GPS information (UBX binary format)
+ * @{
+ *
+ * @file       UBX.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      GPS module, handles GPS and NMEA stream
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "openpilot.h"
+#include "pios.h"
+#include "UBX.h"
+#include "gpsvelocity.h"
+
+bool checksum_ubx_message (UBXPacket *ubx)
+{
+	int i;
+	uint8_t ck_a, ck_b;
+
+	ck_a = ubx->header.class;
+	ck_b = ck_a;
+
+	ck_a += ubx->header.id;
+	ck_b += ck_a;
+
+	ck_a += ubx->header.len & 0xff;
+	ck_b += ck_a;
+
+	ck_a += ubx->header.len >> 8;
+	ck_b += ck_a;
+
+
+
+	for (i = 0; i < ubx->header.len; i++)
+	{
+		ck_a += ubx->payload.payload[i];
+		ck_b += ck_a;
+	}
+
+	if (ubx->header.ck_a == ck_a &&
+			ubx->header.ck_b == ck_b)
+	{
+		return true;
+	}
+	else
+		return false;
+
+}
+
+void parse_ubx_nav_velned (UBXPayload payload)
+{
+	GPSVelocityData GpsVelocity;
+	GPSVelocityGet(&GpsVelocity);
+
+	GpsVelocity.North	= (float)payload.nav_velned.velN/100.0;
+	GpsVelocity.East	= (float)payload.nav_velned.velE/100.0;
+	GpsVelocity.Down	= (float)payload.nav_velned.velD/100.0;
+
+	GPSVelocitySet(&GpsVelocity);
+}
+
+void parse_ubx_message (UBXPacket *ubx)
+{
+	switch (ubx->header.class)
+	{
+		case UBX_CLASS_NAV:
+			switch (ubx->header.id)
+			{
+				case UBX_ID_VELNED:
+					parse_ubx_nav_velned (ubx->payload);
+			}
+			break;
+	}
+}
+
+

flight/Modules/GPS/inc/UBX.h

@@ -0,0 +1,80 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup GSPModule GPS Module
+ * @brief Process GPS information
+ * @{
+ *
+ * @file       UBX.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      GPS module, handles GPS and NMEA stream
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef UBX_H
+#define UBX_H
+#include "openpilot.h"
+
+#define UBX_SYNC1						0xb5 // UBX protocol synchronization characters
+#define UBX_SYNC2						0x62
+
+// From u-blox6 receiver protocol specification
+
+// Messages classes
+#define UBX_CLASS_NAV	0x01
+
+// Message IDs
+#define	UBX_ID_VELNED	0x12
+
+// private structures
+
+typedef struct  {
+	uint32_t	iTOW;	// ms GPS Millisecond Time of Week
+	int32_t		velN;	// cm/s NED north velocity
+	int32_t		velE;	// cm/s NED east velocity
+	int32_t		velD;	// cm/s NED down velocity
+	uint32_t	speed;	// cm/s Speed (3-D)
+	uint32_t	gSpeed;	// cm/s Ground Speed (2-D)
+	int32_t		heading;	// 1e-5 *deg Heading of motion 2-D
+	uint32_t	sAcc;	// cm/s Speed Accuracy Estimate
+	uint32_t	cAcc;	// 1e-5 *deg Course / Heading Accuracy Estimate
+} UBX_NAV_VELNED;
+
+typedef union {		// add more message types later
+	uint8_t		payload[0];
+	UBX_NAV_VELNED	nav_velned;
+} UBXPayload;
+
+typedef struct {
+	uint8_t 	class;
+	uint8_t 	id;
+	uint16_t	len;
+	uint8_t 	ck_a;
+	uint8_t 	ck_b;
+} UBXHeader;
+
+typedef struct {
+	UBXHeader	header;
+	UBXPayload	payload;
+} UBXPacket;
+
+bool checksum_ubx_message(UBXPacket *);
+void parse_ubx_message(UBXPacket *);
+#endif /* UBX_H */

flight/Revolution/UAVObjects.inc

@@ -47,6 +47,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats
 UAVOBJSRCFILENAMES += gpsposition
 UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
+UAVOBJSRCFILENAMES += gpsvelocity
 UAVOBJSRCFILENAMES += guidancesettings
 UAVOBJSRCFILENAMES += homelocation
 UAVOBJSRCFILENAMES += i2cstats

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -54,6 +54,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/gpsposition.h \
     $$UAVOBJECT_SYNTHETICS/gpstime.h \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.h \
+    $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \
     $$UAVOBJECT_SYNTHETICS/positionactual.h \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \
     $$UAVOBJECT_SYNTHETICS/homelocation.h \
@@ -117,6 +118,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \
     $$UAVOBJECT_SYNTHETICS/gpstime.cpp \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
+    $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \
     $$UAVOBJECT_SYNTHETICS/positionactual.cpp \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \
     $$UAVOBJECT_SYNTHETICS/homelocation.cpp \

shared/uavobjectdefinition/gpsvelocity.xml

@@ -0,0 +1,12 @@
+<xml>
+    <object name="GPSVelocity" singleinstance="true" settings="false">
+        <description>Raw GPS data from @ref GPSModule.  Should only be used by @ref AHRSCommsModule.</description>
+        <field name="North" units="m/s" type="float" elements="1"/>
+        <field name="East" units="m/s" type="float" elements="1"/>
+        <field name="Down" units="m/s" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="periodic" period="1000"/>
+    </object>
+</xml>