rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

Experimental extension to the GPS stream parser to extract u-blox

Browse Source 
binary data format. See "u-blox 6 Receiver Description" for
reference. GPS data stream may carry mixed NMEA and UBX data.
This commit is contained in:
Werner Backes 2012-04-09 23:12:24 +02:00
parent 56108b5381
commit 422944ce29
1 changed files with 92 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
flight/Modules/GPS/GPS.c
View File

@ -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: