mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
OP-324 Make NMEA parser more resilient to corrupted messages - WANRING: NMEA parser refactored - VTG and ZDA messages no longer interpreted
Sometimes corrupted NMEA messages can get passed the NMEA "CRC" check and this caused the NEMA parser to crash because it assumed that the correct amount of parameters are present. Now the NMEA message is split into its different parameters before it is passed to the message-specific parser. Hence the function-signature of the parser functions has changed and the implementations of these functions has changed. I also decided to no longer interpret the VTG and ZDA because they do not contain more data than the other messages that are being parsed. Not tested in flight, and could not test the TOP message. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2904 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
FredericG
FredericG
parent
7bfb58ce73
commit
0124c42b8c
@ -230,11 +230,14 @@ static void gpsTask(void *parameters)
|
||||
else
|
||||
if (found_cr && (c == '\n') )
|
||||
{
|
||||
// The NMEA functions require a zero-terminated string
|
||||
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
|
||||
gps_rx_buffer[rx_count-2] = 0;
|
||||
|
||||
// prepare to parse next sentence
|
||||
start_flag = false;
|
||||
found_cr = false;
|
||||
rx_count = 0;
|
||||
|
||||
// Our rxBuffer must look like this now:
|
||||
// [0] = '$'
|
||||
// ... = zero or more bytes of sentence payload
|
||||
@ -246,12 +249,17 @@ static void gpsTask(void *parameters)
|
||||
// Validate the checksum over the sentence
|
||||
if (!NMEA_checksum(&gps_rx_buffer[1]))
|
||||
{ // Invalid checksum. May indicate dropped characters on Rx.
|
||||
PIOS_DEBUG_PinHigh(2);
|
||||
++numChecksumErrors;
|
||||
PIOS_DEBUG_PinLow(2);
|
||||
}
|
||||
else
|
||||
{ // Valid checksum, use this packet to update the GPS position
|
||||
if (!NMEA_update_position(&gps_rx_buffer[1]))
|
||||
if (!NMEA_update_position(&gps_rx_buffer[1])) {
|
||||
PIOS_DEBUG_PinHigh(2);
|
||||
++numParsingErrors;
|
||||
PIOS_DEBUG_PinLow(2);
|
||||
}
|
||||
else
|
||||
++numUpdates;
|
||||
|
||||
|
||||
@ -35,38 +35,50 @@
|
||||
#include "gpstime.h"
|
||||
#include "gpssatellites.h"
|
||||
|
||||
//#define ENABLE_DEBUG_MSG ///< define to enable debug-messages
|
||||
#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages
|
||||
|
||||
|
||||
|
||||
#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP)
|
||||
|
||||
// Debugging
|
||||
//#define GPSDEBUG
|
||||
#ifdef GPSDEBUG
|
||||
#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
|
||||
#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
|
||||
#define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages
|
||||
#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages
|
||||
#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
|
||||
#define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages
|
||||
#define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages
|
||||
#define NMEA_DEBUG_PGTOP ///< define to enable debug of PGTOP messages
|
||||
#ifdef ENABLE_DEBUG_MSG
|
||||
//#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages
|
||||
//#define DEBUG_PARS ///< define to display the incoming NMEA messages split into its parameters
|
||||
//#define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages
|
||||
//#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
|
||||
//#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
|
||||
//#define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages
|
||||
//#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages
|
||||
//#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
|
||||
//#define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages
|
||||
//#define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages
|
||||
//#define NMEA_DEBUG_PGTOP ///< define to enable debug of PGTOP messages
|
||||
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
|
||||
#else
|
||||
#define DEBUG_MSG(format, ...)
|
||||
#endif
|
||||
|
||||
#define MAX_NB_PARAMS 20
|
||||
/* NMEA sentence parsers */
|
||||
|
||||
struct nmea_parser {
|
||||
const char *prefix;
|
||||
bool(*handler) (GPSPositionData * GpsData, char *sentence);
|
||||
bool(*handler) (GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
uint32_t cnt;
|
||||
};
|
||||
|
||||
#ifdef ENABLE_GPS_NMEA
|
||||
static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence);
|
||||
static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence);
|
||||
static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence);
|
||||
static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence);
|
||||
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, char *sentence);
|
||||
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence);
|
||||
static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
#endif
|
||||
|
||||
static bool nmeaProcessPGTOP(GPSPositionData * GpsData, char *sentence);
|
||||
static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
|
||||
|
||||
static struct nmea_parser nmea_parsers[] = {
|
||||
|
||||
@ -74,35 +86,42 @@ struct nmea_parser {
|
||||
{
|
||||
.prefix = "GPGGA",
|
||||
.handler = nmeaProcessGPGGA,
|
||||
.cnt = 0,
|
||||
},
|
||||
{
|
||||
.prefix = "GPVTG",
|
||||
.handler = nmeaProcessGPVTG,
|
||||
.cnt = 0,
|
||||
},
|
||||
{
|
||||
.prefix = "GPGSA",
|
||||
.handler = nmeaProcessGPGSA,
|
||||
.cnt = 0,
|
||||
},
|
||||
{
|
||||
.prefix = "GPRMC",
|
||||
.handler = nmeaProcessGPRMC,
|
||||
.cnt = 0,
|
||||
},
|
||||
{
|
||||
.prefix = "GPZDA",
|
||||
.handler = nmeaProcessGPZDA,
|
||||
.cnt = 0,
|
||||
},
|
||||
{
|
||||
.prefix = "GPGSV",
|
||||
.handler = nmeaProcessGPGSV,
|
||||
.cnt = 0,
|
||||
},
|
||||
#endif
|
||||
{
|
||||
.prefix = "PGTOP",
|
||||
.handler = nmeaProcessPGTOP,
|
||||
.cnt = 0,
|
||||
},
|
||||
};
|
||||
|
||||
static struct nmea_parser *NMEA_find_parser_by_prefix(char *prefix)
|
||||
static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
|
||||
{
|
||||
if (!prefix) {
|
||||
return (NULL);
|
||||
@ -216,7 +235,7 @@ static float NMEA_real_to_float(char *nmea_real)
|
||||
* DD[D]MM.mmmm[mm]
|
||||
* into a fixed-point representation in units of (degrees * 1e-7)
|
||||
*/
|
||||
static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon)
|
||||
static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool negative)
|
||||
{
|
||||
int32_t num_DDDMM;
|
||||
uint32_t num_m;
|
||||
@ -263,10 +282,14 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon)
|
||||
*latlon += (num_DDDMM % 100) * 1e7 / 60; /* add in the scaled decimal whole minutes */
|
||||
*latlon += num_m / 60; /* add in the scaled decimal fractional minutes */
|
||||
|
||||
if (negative)
|
||||
*latlon *= -1;
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif // ENABLE_GPS_NMEA
|
||||
|
||||
|
||||
/**
|
||||
* Parses a complete NMEA sentence and updates the GPSPosition UAVObject
|
||||
* \param[in] An NMEA sentence with a valid checksum
|
||||
@ -275,40 +298,75 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon)
|
||||
*/
|
||||
bool NMEA_update_position(char *nmea_sentence)
|
||||
{
|
||||
char *sentence = nmea_sentence;
|
||||
char* p = nmea_sentence;
|
||||
char* params[MAX_NB_PARAMS];
|
||||
uint8_t nbParams;
|
||||
|
||||
#ifdef DEBUG_MSG_IN
|
||||
DEBUG_MSG("\"%s\"\n", nmea_sentence);
|
||||
#endif
|
||||
|
||||
// Split the nmea sentence it its parameters, separated by ","
|
||||
// Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43"
|
||||
|
||||
// The first parameter starts at the beginning of the message
|
||||
params[0] = p;
|
||||
nbParams = 1;
|
||||
while (*p != 0) {
|
||||
if (*p == '*') {
|
||||
// After the * comes the "CRC", we are done,
|
||||
*p = 0; // Zero-terminate this parameter
|
||||
break;
|
||||
} else if (*p == ',') {
|
||||
// This is the end of this parameter
|
||||
*p = 0; // Zero-terminate this parameter
|
||||
// Start new parameter
|
||||
if (nbParams==MAX_NB_PARAMS)
|
||||
break;
|
||||
params[nbParams] = p+1; // For sure there is something at p+1 because at p there is ","
|
||||
nbParams++;
|
||||
}
|
||||
p++;
|
||||
}
|
||||
|
||||
|
||||
#ifdef DEBUG_PARAMS
|
||||
int i;
|
||||
for (i=0;i<nbParams; i++) {
|
||||
DEBUG_MSG(" %d \"%s\"\n", i, params[i]);
|
||||
}
|
||||
#endif
|
||||
|
||||
// The first parameter is the message name, lets see if we find a parser for it
|
||||
struct nmea_parser *parser;
|
||||
char *prefix;
|
||||
|
||||
/* Find out what kind of NMEA packet we're dealing with */
|
||||
prefix = strsep(&sentence, ",");
|
||||
|
||||
/* Check if we have a parser for this packet type */
|
||||
parser = NMEA_find_parser_by_prefix(prefix);
|
||||
|
||||
parser = NMEA_find_parser_by_prefix(params[0]);
|
||||
if (!parser) {
|
||||
/* Valid but unhandled packet type */
|
||||
// No parser found
|
||||
DEBUG_MSG(" NO PARSER (\"%s\")\n", params[0]);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Found a matching parser for this packet type */
|
||||
|
||||
/* Reject empty (but valid) packets without parsing */
|
||||
if (sentence[0] == ',' && sentence[1] == ',' && sentence[2] == ',') {
|
||||
/* Nothing to parse, */
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Parse the sentence and update the GpsData object */
|
||||
|
||||
parser->cnt++;
|
||||
#ifdef DEBUG_MGSID_IN
|
||||
DEBUG_MSG("%s %d\n", params[0], parser->cnt);
|
||||
#endif
|
||||
// Send the message to then parser and get it update the GpsData
|
||||
GPSPositionData GpsData;
|
||||
GPSPositionGet(&GpsData);
|
||||
if (!parser->handler(&GpsData, sentence)) {
|
||||
/* Parse failed for valid checksum. Do not update the UAVObject. */
|
||||
bool gpsDataUpdated;
|
||||
|
||||
if (!parser->handler(&GpsData, &gpsDataUpdated, params, nbParams)) {
|
||||
// Parse failed
|
||||
DEBUG_MSG(" PARSE FAILED (\"%s\")\n", params[0]);
|
||||
return false;
|
||||
}
|
||||
GPSPositionSet(&GpsData);
|
||||
|
||||
/* Tell the caller what kind of packet we just parsed */
|
||||
|
||||
// All is fine :) Update object if data has changed
|
||||
if (gpsDataUpdated) {
|
||||
GPSPositionSet(&GpsData);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -319,83 +377,43 @@ bool NMEA_update_position(char *nmea_sentence)
|
||||
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
|
||||
* \param[in] An NMEA sentence with a valid checksum
|
||||
*/
|
||||
static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence)
|
||||
static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
|
||||
{
|
||||
char *next = sentence;
|
||||
char *tokens;
|
||||
char *delimiter = ",*";
|
||||
|
||||
#ifdef NMEA_DEBUG_GGA
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
|
||||
if (nbParam != 15)
|
||||
return false;
|
||||
|
||||
#ifdef NMEA_DEBUG_GGA
|
||||
DEBUG_MSG("\n UTC=%s\n", param[1]);
|
||||
DEBUG_MSG(" Lat=%s %s\n", param[2], param[3]);
|
||||
DEBUG_MSG(" Long=%s %s\n", param[4], param[5]);
|
||||
DEBUG_MSG(" Fix=%s\n", param[6]);
|
||||
DEBUG_MSG(" Sat=%s\n", param[7]);
|
||||
DEBUG_MSG(" HDOP=%s\n", param[8]);
|
||||
DEBUG_MSG(" Alt=%s %s\n", param[9], param[10]);
|
||||
DEBUG_MSG(" GeoidSep=%s %s\n\n", param[11]);
|
||||
#endif
|
||||
|
||||
// get UTC time [hhmmss.sss]
|
||||
tokens = strsep(&next, delimiter);
|
||||
//strcpy(GpsInfo.TimeOfFix,tokens);
|
||||
*gpsDataUpdated = true;
|
||||
|
||||
// next field: latitude
|
||||
// get latitude [DDMM.mmmmm]
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, tokens)) {
|
||||
// get latitude [DDMM.mmmmm] [N|S]
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) {
|
||||
return false;
|
||||
}
|
||||
// next field: N/S indicator
|
||||
// correct latitude for N/S
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (tokens[0] == 'S')
|
||||
GpsData->Latitude = -GpsData->Latitude;
|
||||
|
||||
// next field: longitude
|
||||
// get longitude [dddmm.mmmmm]
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, tokens)) {
|
||||
// get longitude [dddmm.mmmmm] [E|W]
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[4], param[5][0] == 'W')) {
|
||||
return false;
|
||||
}
|
||||
// next field: E/W indicator
|
||||
// correct longitude for E/W
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (tokens[0] == 'W')
|
||||
GpsData->Longitude = -GpsData->Longitude;
|
||||
|
||||
// next field: position fix status
|
||||
// position fix status
|
||||
// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
|
||||
// check for good position fix
|
||||
tokens = strsep(&next, delimiter);
|
||||
//if((tokens[0] != '0') || (tokens[0] != 0))
|
||||
// GpsData.Updates++;
|
||||
|
||||
// next field: satellites used
|
||||
// get number of satellites used in GPS solution
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Satellites = atoi(tokens);
|
||||
GpsData->Satellites = atoi(param[7]);
|
||||
|
||||
// next field: HDOP (horizontal dilution of precision)
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: altitude
|
||||
// get altitude (in meters mm.m)
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Altitude = NMEA_real_to_float(tokens);
|
||||
GpsData->Altitude = NMEA_real_to_float(param[9]);
|
||||
|
||||
// next field: altitude units, always 'M'
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: geoid separation
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->GeoidSeparation = NMEA_real_to_float(tokens);
|
||||
|
||||
// next field: separation units
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: DGPS age
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: DGPS station ID
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: checksum
|
||||
tokens = strsep(&next, delimiter);
|
||||
// geoid separation
|
||||
GpsData->GeoidSeparation = NMEA_real_to_float(param[11]);
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -405,87 +423,56 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence)
|
||||
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
|
||||
* \param[in] An NMEA sentence with a valid checksum
|
||||
*/
|
||||
static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence)
|
||||
static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
|
||||
{
|
||||
char *next = sentence;
|
||||
char *tokens;
|
||||
char *delimiter = ",*";
|
||||
if (nbParam != 13)
|
||||
return false;
|
||||
|
||||
#ifdef NMEA_DEBUG_RMC
|
||||
DEBUG_MSG("\n UTC=%s\n", param[1]);
|
||||
DEBUG_MSG(" Lat=%s %s\n", param[3], param[4]);
|
||||
DEBUG_MSG(" Long=%s %s\n", param[5], param[6]);
|
||||
DEBUG_MSG(" Speed=%s\n", param[7]);
|
||||
DEBUG_MSG(" Course=%s\n", param[8]);
|
||||
DEBUG_MSG(" DateOfFix=%s\n\n", param[9]);
|
||||
#endif
|
||||
|
||||
*gpsDataUpdated = true;
|
||||
|
||||
GPSTimeData gpst;
|
||||
GPSTimeGet(&gpst);
|
||||
|
||||
#ifdef NMEA_DEBUG_RMC
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
|
||||
#endif
|
||||
|
||||
// get UTC time [hhmmss.sss]
|
||||
tokens = strsep(&next, delimiter);
|
||||
float hms = NMEA_real_to_float(tokens);
|
||||
float hms = NMEA_real_to_float(param[1]);
|
||||
gpst.Second = (int)hms % 100;
|
||||
gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
|
||||
gpst.Hour = (int)hms / 10000;
|
||||
|
||||
// next field: Navigation receiver warning A = OK, V = warning
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: latitude
|
||||
// get latitude [ddmm.mmmmm]
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, tokens)) {
|
||||
// get latitude [DDMM.mmmmm] [N|S]
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) {
|
||||
return false;
|
||||
}
|
||||
// next field: N/S indicator
|
||||
// correct latitude for N/S
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (tokens[0] == 'S')
|
||||
GpsData->Latitude = -GpsData->Latitude;
|
||||
|
||||
// next field: longitude
|
||||
// get longitude [dddmm.mmmmm]
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, tokens)) {
|
||||
// get longitude [dddmm.mmmmm] [E|W]
|
||||
if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[5], param[6][0] == 'W')) {
|
||||
return false;
|
||||
}
|
||||
// next field: E/W indicator
|
||||
// correct longitude for E/W
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (tokens[0] == 'W')
|
||||
GpsData->Longitude = -GpsData->Longitude;
|
||||
|
||||
// next field: speed (knots)
|
||||
// get speed in knots
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Groundspeed = NMEA_real_to_float(tokens);
|
||||
// to m/s
|
||||
GpsData->Groundspeed *= 0.51444;
|
||||
GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444; // to m/s
|
||||
|
||||
// next field: True course
|
||||
// get True course
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Heading = NMEA_real_to_float(tokens);
|
||||
GpsData->Heading = NMEA_real_to_float(param[8]);
|
||||
|
||||
// next field: Date of fix
|
||||
// get Date of fix
|
||||
tokens = strsep(&next, delimiter);
|
||||
// TODO: Should really not use a float here to be safe
|
||||
float date = NMEA_real_to_float(tokens);
|
||||
float date = NMEA_real_to_float(param[9]);
|
||||
gpst.Year = (int)date % 100;
|
||||
gpst.Month = (((int)date - gpst.Year) / 100) % 100;
|
||||
gpst.Day = (int)(date / 10000);
|
||||
gpst.Year += 2000;
|
||||
|
||||
// next field: Magnetic variation
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: E or W
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: Mode: A=autonomous, D=differential, E=Estimated, N=not valid, S=Simulator
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: checksum
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
GPSTimeSet(&gpst);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -494,49 +481,10 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence)
|
||||
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
|
||||
* \param[in] An NMEA sentence with a valid checksum
|
||||
*/
|
||||
static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence)
|
||||
static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
|
||||
{
|
||||
char *next = sentence;
|
||||
char *tokens;
|
||||
char *delimiter = ",*";
|
||||
|
||||
#ifdef NMEA_DEBUG_VTG
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
|
||||
#endif
|
||||
|
||||
// get course (true north ref) in degrees [ddd.dd]
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Heading = NMEA_real_to_float(tokens);
|
||||
|
||||
// next field: 'T'
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: course (magnetic north)
|
||||
// get course (magnetic north ref) in degrees [ddd.dd]
|
||||
tokens = strsep(&next, delimiter);
|
||||
// next field: 'M'
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: speed (knots)
|
||||
// get speed in knots
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Groundspeed = NMEA_real_to_float(tokens);
|
||||
// to m/s
|
||||
GpsData->Groundspeed *= 0.51444;
|
||||
|
||||
// next field: 'N'
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: speed (km/h)
|
||||
// get speed in km/h
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: 'K'
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: checksum
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// No new data data extracted
|
||||
*gpsDataUpdated = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -545,37 +493,10 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence)
|
||||
* \param[in] A pointer to a GPSPosition UAVObject to be updated (unused).
|
||||
* \param[in] An NMEA sentence with a valid checksum
|
||||
*/
|
||||
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, char *sentence)
|
||||
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
|
||||
{
|
||||
char *next = sentence;
|
||||
char *tokens;
|
||||
char *delimiter = ",*";
|
||||
|
||||
#ifdef NMEA_DEBUG_ZDA
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
|
||||
#endif
|
||||
|
||||
GPSTimeData gpst;
|
||||
GPSTimeGet(&gpst);
|
||||
|
||||
tokens = strsep(&next, delimiter);
|
||||
float hms = NMEA_real_to_float(tokens);
|
||||
|
||||
gpst.Second = (int)hms % 100;
|
||||
gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
|
||||
gpst.Hour = (int)hms / 10000;
|
||||
|
||||
tokens = strsep(&next, delimiter);
|
||||
gpst.Day = atoi(tokens);
|
||||
|
||||
tokens = strsep(&next, delimiter);
|
||||
gpst.Month = atoi(tokens);
|
||||
|
||||
tokens = strsep(&next, delimiter);
|
||||
gpst.Year = atoi(tokens);
|
||||
|
||||
GPSTimeSet(&gpst);
|
||||
|
||||
// No new data data extracted
|
||||
*gpsDataUpdated = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -587,51 +508,38 @@ static uint8_t gsv_processed_mask;
|
||||
static uint16_t gsv_incomplete_error;
|
||||
static uint16_t gsv_duplicate_error;
|
||||
|
||||
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence)
|
||||
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
|
||||
{
|
||||
char *next = sentence;
|
||||
char *tokens;
|
||||
char *delimiter = ",";
|
||||
if (nbParam < 4)
|
||||
return false;
|
||||
|
||||
#ifdef NMEA_DEBUG_GSV
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
|
||||
DEBUG_MSG("\n Sentence=%s/%s\n", param[2], param[1]);
|
||||
DEBUG_MSG(" Sats=%s\n", param[3]);
|
||||
#endif
|
||||
|
||||
/* Drop the checksum */
|
||||
char *tmp = sentence;
|
||||
char *tmp_delim = "*";
|
||||
next = strsep(&tmp, tmp_delim);
|
||||
uint8_t nbSentences = atoi(param[1]);
|
||||
uint8_t currSentence = atoi(param[2]);
|
||||
|
||||
/* # of sentences in full GSV data set */
|
||||
tokens = strsep(&next, delimiter);
|
||||
uint8_t total_sentences = atoi(tokens);
|
||||
if ((total_sentences < 1) || (total_sentences > 8)) {
|
||||
*gpsDataUpdated = true;
|
||||
|
||||
if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences)
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Sentence number within the current GSV data set */
|
||||
tokens = strsep(&next, delimiter);
|
||||
uint8_t current_sentence = atoi(tokens);
|
||||
if (current_sentence < 1) {
|
||||
return false;
|
||||
}
|
||||
gsv_partial.SatsInView = atoi(param[3]);
|
||||
|
||||
/* # of satellites currently in view */
|
||||
tokens = strsep(&next, delimiter);
|
||||
gsv_partial.SatsInView = atoi(tokens);
|
||||
|
||||
/* Find out if this is the first sentence in the GSV set */
|
||||
if (current_sentence == 1) {
|
||||
// Find out if this is the first sentence in the GSV set
|
||||
if (currSentence == 1) {
|
||||
if (gsv_expected_mask != gsv_processed_mask) {
|
||||
/* We are starting over when we haven't yet finished our previous GSV group */
|
||||
// We are starting over when we haven't yet finished our previous GSV group
|
||||
gsv_incomplete_error++;
|
||||
}
|
||||
|
||||
/* First GSV sentence in the sequence, reset our expected_mask */
|
||||
gsv_expected_mask = (1 << total_sentences) - 1;
|
||||
// First GSV sentence in the sequence, reset our expected_mask
|
||||
gsv_expected_mask = (1 << nbSentences) - 1;
|
||||
}
|
||||
|
||||
uint8_t current_sentence_id = (1 << (current_sentence - 1));
|
||||
uint8_t current_sentence_id = (1 << (currSentence - 1));
|
||||
if (gsv_processed_mask & current_sentence_id) {
|
||||
/* Duplicate sentence in this GSV set */
|
||||
gsv_duplicate_error++;
|
||||
@ -640,29 +548,32 @@ static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence)
|
||||
gsv_processed_mask |= current_sentence_id;
|
||||
}
|
||||
|
||||
uint8_t parIdx = 4;
|
||||
|
||||
#ifdef NMEA_DEBUG_GSV
|
||||
DEBUG_MSG(" PRN:");
|
||||
#endif
|
||||
|
||||
/* Make sure this sentence can fit in our GPSSatellites object */
|
||||
if ((current_sentence * 4) <= NELEMENTS(gsv_partial.PRN)) {
|
||||
if ((currSentence * 4) <= NELEMENTS(gsv_partial.PRN)) {
|
||||
/* Process 4 blocks of satellite info */
|
||||
for (uint8_t i = 0; next && i < 4; i++) {
|
||||
uint8_t sat_index = ((current_sentence - 1) * 4) + i;
|
||||
for (uint8_t i = 0; parIdx+4 <= nbParam && i < 4; i++) {
|
||||
uint8_t sat_index = ((currSentence - 1) * 4) + i;
|
||||
|
||||
/* PRN number */
|
||||
tokens = strsep(&next, delimiter);
|
||||
gsv_partial.PRN[sat_index] = atoi(tokens);
|
||||
|
||||
/* Elevation */
|
||||
tokens = strsep(&next, delimiter);
|
||||
gsv_partial.Elevation[sat_index] = NMEA_real_to_float(tokens);
|
||||
|
||||
/* Azimuth */
|
||||
tokens = strsep(&next, delimiter);
|
||||
gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(tokens);
|
||||
|
||||
/* SNR */
|
||||
tokens = strsep(&next, delimiter);
|
||||
gsv_partial.SNR[sat_index] = atoi(tokens);
|
||||
// Get sat info
|
||||
gsv_partial.PRN[sat_index] = atoi(param[parIdx++]);
|
||||
gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]);
|
||||
gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]);
|
||||
gsv_partial.SNR[sat_index] = atoi(param[parIdx++]);
|
||||
#ifdef NMEA_DEBUG_GSV
|
||||
DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#ifdef NMEA_DEBUG_GSV
|
||||
DEBUG_MSG("\n");
|
||||
#endif
|
||||
|
||||
|
||||
/* Find out if we're finished processing all GSV sentences in the set */
|
||||
if ((gsv_expected_mask != 0) && (gsv_processed_mask == gsv_expected_mask)) {
|
||||
@ -681,24 +592,21 @@ static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence)
|
||||
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
|
||||
* \param[in] An NMEA sentence with a valid checksum
|
||||
*/
|
||||
static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence)
|
||||
static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
|
||||
{
|
||||
char *next = sentence;
|
||||
char *tokens;
|
||||
char *delimiter = ",*";
|
||||
if (nbParam != 18)
|
||||
return false;
|
||||
|
||||
#ifdef NMEA_DEBUG_GSA
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
|
||||
DEBUG_MSG("\n Status=%s\n", param[2]);
|
||||
DEBUG_MSG(" PDOP=%s\n", param[15]);
|
||||
DEBUG_MSG(" HDOP=%s\n", param[16]);
|
||||
DEBUG_MSG(" VDOP=%s\n", param[17]);
|
||||
#endif
|
||||
|
||||
// next field: Mode
|
||||
// Mode: M=Manual, forced to operate in 2D or 3D, A=Automatic, 3D/2D
|
||||
tokens = strsep(&next, delimiter);
|
||||
*gpsDataUpdated = true;
|
||||
|
||||
// next field: Mode
|
||||
// Mode: 1=Fix not available, 2=2D, 3=3D
|
||||
tokens = strsep(&next, delimiter);
|
||||
switch (atoi(tokens)) {
|
||||
switch (atoi(param[2])) {
|
||||
case 1:
|
||||
GpsData->Status = GPSPOSITION_STATUS_NOFIX;
|
||||
break;
|
||||
@ -714,34 +622,14 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence)
|
||||
break;
|
||||
}
|
||||
|
||||
// next field: 3-14 IDs of SVs used in position fix (null for unused fields)
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
// next field: PDOP
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->PDOP = NMEA_real_to_float(tokens);
|
||||
GpsData->PDOP = NMEA_real_to_float(param[15]);
|
||||
|
||||
// next field: HDOP
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->HDOP = NMEA_real_to_float(tokens);
|
||||
GpsData->HDOP = NMEA_real_to_float(param[16]);
|
||||
|
||||
// next field: VDOP
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->VDOP = NMEA_real_to_float(tokens);
|
||||
|
||||
// next field: checksum
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->VDOP = NMEA_real_to_float(param[17]);
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -753,114 +641,76 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence)
|
||||
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
|
||||
* \param[in] An NMEA sentence with a valid checksum
|
||||
*/
|
||||
static bool nmeaProcessPGTOP(GPSPositionData * GpsData, char *sentence)
|
||||
static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
|
||||
{
|
||||
char *next = sentence;
|
||||
char *tokens;
|
||||
char *delimiter = ",*";
|
||||
if (nbParam != 17)
|
||||
return false;
|
||||
|
||||
#ifdef NMEA_DEBUG_PGTOP
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
|
||||
#endif
|
||||
GPSTimeData gpst;
|
||||
GPSTimeGet(&gpst);
|
||||
GPSTimeData gpst;
|
||||
GPSTimeGet(&gpst);
|
||||
|
||||
// get UTC time [hhmmss.sss]
|
||||
tokens = strsep(&next, delimiter);
|
||||
float hms = NMEA_real_to_float(tokens);
|
||||
gpst.Second = (int)hms % 100;
|
||||
gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
|
||||
gpst.Hour = (int)hms / 10000;
|
||||
*gpsDataUpdated = true;
|
||||
|
||||
// next field: latitude
|
||||
// get latitude decimal degrees
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Latitude = NMEA_real_to_float(tokens)*1e7;
|
||||
// get UTC time [hhmmss.sss]
|
||||
float hms = NMEA_real_to_float(param[1]);
|
||||
gpst.Second = (int)hms % 100;
|
||||
gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
|
||||
gpst.Hour = (int)hms / 10000;
|
||||
|
||||
// next field: N/S indicator
|
||||
// correct latitude for N/S
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (tokens[0] == 'S')
|
||||
GpsData->Latitude = -GpsData->Latitude;
|
||||
// get latitude decimal degrees
|
||||
GpsData->Latitude = NMEA_real_to_float(param[2])*1e7;
|
||||
if (param[3][0] == 'S')
|
||||
GpsData->Latitude = -GpsData->Latitude;
|
||||
|
||||
// next field: longitude
|
||||
// get longitude decimal degrees
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Longitude = NMEA_real_to_float(tokens)*1e7;
|
||||
|
||||
// next field: E/W indicator
|
||||
// correct longitude for E/W
|
||||
tokens = strsep(&next, delimiter);
|
||||
if (tokens[0] == 'W')
|
||||
GpsData->Longitude = -GpsData->Longitude;
|
||||
// get longitude decimal degrees
|
||||
GpsData->Longitude = NMEA_real_to_float(param[4])*1e7;
|
||||
if (param[5][0] == 'W')
|
||||
GpsData->Longitude = -GpsData->Longitude;
|
||||
|
||||
// next field: Fix Quality
|
||||
// Mode: 0=Fix not available, 1=GPS fix, 2=DGPS fix
|
||||
tokens = strsep(&next, delimiter);
|
||||
// get number of satellites used in GPS solution
|
||||
GpsData->Satellites = atoi(param[7]);
|
||||
|
||||
// next field: satellites used
|
||||
// get number of satellites used in GPS solution
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Satellites = atoi(tokens);
|
||||
// next field: HDOP
|
||||
GpsData->HDOP = NMEA_real_to_float(param[8]);
|
||||
|
||||
// next field: HDOP
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->HDOP = NMEA_real_to_float(tokens);
|
||||
// get altitude (in meters mm.m)
|
||||
GpsData->Altitude = NMEA_real_to_float(param[9]);
|
||||
|
||||
// next field: altitude
|
||||
// get altitude (in meters mm.m)
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Altitude = NMEA_real_to_float(tokens);
|
||||
// next field: geoid separation
|
||||
GpsData->GeoidSeparation = NMEA_real_to_float(param[10]);
|
||||
|
||||
// next field: geoid separation
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->GeoidSeparation = NMEA_real_to_float(tokens);
|
||||
// Mode: 1=Fix not available, 2=2D, 3=3D
|
||||
switch (atoi(param[11])) {
|
||||
case 1:
|
||||
GpsData->Status = GPSPOSITION_STATUS_NOFIX;
|
||||
break;
|
||||
case 2:
|
||||
GpsData->Status = GPSPOSITION_STATUS_FIX2D;
|
||||
break;
|
||||
case 3:
|
||||
GpsData->Status = GPSPOSITION_STATUS_FIX3D;
|
||||
break;
|
||||
default:
|
||||
/* Unhandled */
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
// next field: Fix Type
|
||||
// Mode: 1=Fix not available, 2=2D, 3=3D
|
||||
tokens = strsep(&next, delimiter);
|
||||
switch (atoi(tokens)) {
|
||||
case 1:
|
||||
GpsData->Status = GPSPOSITION_STATUS_NOFIX;
|
||||
break;
|
||||
case 2:
|
||||
GpsData->Status = GPSPOSITION_STATUS_FIX2D;
|
||||
break;
|
||||
case 3:
|
||||
GpsData->Status = GPSPOSITION_STATUS_FIX3D;
|
||||
break;
|
||||
default:
|
||||
/* Unhandled */
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
// get course over ground in degrees [ddd.dd]
|
||||
GpsData->Heading = NMEA_real_to_float(param[12]);
|
||||
|
||||
// get course over ground in degrees [ddd.dd]
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Heading = NMEA_real_to_float(tokens);
|
||||
// get speed in km/h
|
||||
GpsData->Groundspeed = NMEA_real_to_float(param[13]);
|
||||
// to m/s
|
||||
GpsData->Groundspeed /= 3.6;
|
||||
|
||||
// next field: speed (km/h)
|
||||
// get speed in km/h
|
||||
tokens = strsep(&next, delimiter);
|
||||
GpsData->Groundspeed = NMEA_real_to_float(tokens);
|
||||
// to m/s
|
||||
GpsData->Groundspeed /= 3.6;
|
||||
gpst.Day = atoi(param[14]);
|
||||
gpst.Month = atoi(param[15]);
|
||||
gpst.Year = atoi(param[16]);
|
||||
GPSTimeSet(&gpst);
|
||||
|
||||
tokens = strsep(&next, delimiter);
|
||||
gpst.Day = atoi(tokens);
|
||||
|
||||
tokens = strsep(&next, delimiter);
|
||||
gpst.Month = atoi(tokens);
|
||||
|
||||
tokens = strsep(&next, delimiter);
|
||||
gpst.Year = atoi(tokens);
|
||||
|
||||
GPSTimeSet(&gpst);
|
||||
|
||||
// next field: checksum
|
||||
tokens = strsep(&next, delimiter);
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // #if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user