mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-28 17:54:15 +01:00
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.
This commit is contained in:
parent
56108b5381
commit
422944ce29
@ -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:
|
||||
|
Loading…
x
Reference in New Issue
Block a user