mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-04 12:24:11 +01:00
802 lines
25 KiB
C
802 lines
25 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup GSPModule GPS Module
|
|
* @brief Process GPS information
|
|
* @{
|
|
*
|
|
* @file NMEA.c
|
|
* @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
|
|
*/
|
|
|
|
#include "openpilot.h"
|
|
#include "pios.h"
|
|
|
|
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
|
|
|
|
#include "gpspositionsensor.h"
|
|
#include "NMEA.h"
|
|
#include "gpstime.h"
|
|
#include "gpssatellites.h"
|
|
#include "GPS.h"
|
|
|
|
// #define ENABLE_DEBUG_MSG ///< define to enable debug-messages
|
|
#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is used for debug-messages
|
|
|
|
// Debugging
|
|
#ifdef ENABLE_DEBUG_MSG
|
|
// #define DEBUG_MSG_IN ///< define to display the incoming NMEA messages
|
|
// #define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters
|
|
// #define DEBUG_MSGID_IN ///< define to display 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 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)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
|
|
};
|
|
|
|
static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
|
|
static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
|
|
static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
|
|
static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
|
|
#if !defined(PIOS_GPS_MINIMAL)
|
|
static bool nmeaProcessGxZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
|
|
static bool nmeaProcessGxGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
|
|
#endif // PIOS_GPS_MINIMAL
|
|
|
|
static const struct nmea_parser nmea_parsers[] = {
|
|
{
|
|
.prefix = "GGA",
|
|
.handler = nmeaProcessGxGGA,
|
|
},
|
|
{
|
|
.prefix = "VTG",
|
|
.handler = nmeaProcessGxVTG,
|
|
},
|
|
{
|
|
.prefix = "GSA",
|
|
.handler = nmeaProcessGxGSA,
|
|
},
|
|
{
|
|
.prefix = "RMC",
|
|
.handler = nmeaProcessGxRMC,
|
|
},
|
|
#if !defined(PIOS_GPS_MINIMAL)
|
|
{
|
|
.prefix = "ZDA",
|
|
.handler = nmeaProcessGxZDA,
|
|
},
|
|
{
|
|
.prefix = "GSV",
|
|
.handler = nmeaProcessGxGSV,
|
|
},
|
|
#endif // PIOS_GPS_MINIMAL
|
|
};
|
|
|
|
int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
|
|
{
|
|
static uint8_t rx_count = 0;
|
|
static bool start_flag = false;
|
|
static bool found_cr = false;
|
|
bool goodParse = false;
|
|
uint8_t c;
|
|
int i = 0;
|
|
|
|
while (i < len) {
|
|
c = rx[i++];
|
|
// detect start while acquiring stream
|
|
// if we find a $ in the middle it was a bad packet (e.g. maybe UBX binary),
|
|
// and this may be the start of another packet
|
|
// silently cancel the current sentence
|
|
if (c == '$') { // NMEA identifier found
|
|
start_flag = false;
|
|
}
|
|
if (!start_flag) { // if no NMEA identifier ('$') found yet
|
|
if (c == '$') { // NMEA identifier found
|
|
start_flag = true;
|
|
found_cr = false;
|
|
rx_count = 0;
|
|
} else {
|
|
// find a likely candidate for a NMEA string
|
|
// skip over some e.g. uBlox packets
|
|
uint8_t *p;
|
|
p = memchr(&rx[i], '$', len - i);
|
|
if (p) {
|
|
i += p - &rx[i];
|
|
} else {
|
|
i = len;
|
|
}
|
|
// loop to restart at the $ if there is one
|
|
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.
|
|
gpsRxStats->gpsRxOverflow++;
|
|
start_flag = false;
|
|
continue;
|
|
} else {
|
|
gps_rx_buffer[rx_count++] = c;
|
|
}
|
|
|
|
// look for ending '\r\n' sequence
|
|
if (!found_cr && (c == '\r')) {
|
|
found_cr = true;
|
|
} else if (found_cr) {
|
|
if (c != '\n') {
|
|
found_cr = false; // false end flag
|
|
} else {
|
|
// 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;
|
|
// Our rxBuffer must look like this now:
|
|
// [0] = '$'
|
|
// ... = zero or more bytes of sentence payload
|
|
// [end_pos - 1] = '\r'
|
|
// [end_pos] = '\n'
|
|
//
|
|
// Prepare to consume the sentence from the buffer
|
|
|
|
// 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);
|
|
gpsRxStats->gpsRxChkSumError++;
|
|
// PIOS_DEBUG_PinLow(2);
|
|
} else { // Valid checksum, use this packet to update the GPS position
|
|
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
|
|
// PIOS_DEBUG_PinHigh(2);
|
|
gpsRxStats->gpsRxParserError++;
|
|
// PIOS_DEBUG_PinLow(2);
|
|
} else {
|
|
gpsRxStats->gpsRxReceived++;
|
|
goodParse = true;
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (goodParse) {
|
|
// if so much as one good sentence we return a good status so the connection status says "alive"
|
|
// if we didn't do this, a lot of garbage (e.g. UBX protocol) mixed in with enough NMEA to fly
|
|
// might think the GPS was offline
|
|
return PARSER_COMPLETE;
|
|
} else {
|
|
return PARSER_INCOMPLETE;
|
|
}
|
|
}
|
|
|
|
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
|
|
{
|
|
if (!prefix) {
|
|
return NULL;
|
|
}
|
|
|
|
for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) {
|
|
const struct nmea_parser *parser = &nmea_parsers[i];
|
|
|
|
/* Use strcmp to check for exact equality over the entire prefix */
|
|
if (!strcmp(prefix, parser->prefix)) {
|
|
/* Found an appropriate parser */
|
|
return parser;
|
|
}
|
|
}
|
|
|
|
/* No matching parser for this prefix */
|
|
return NULL;
|
|
}
|
|
|
|
/**
|
|
* Computes NMEA sentence checksum
|
|
* \param[in] Buffer for parsed nmea sentence
|
|
* \return false checksum not valid
|
|
* \return true checksum valid
|
|
*/
|
|
bool NMEA_checksum(char *nmea_sentence)
|
|
{
|
|
uint8_t checksum_computed = 0;
|
|
uint8_t checksum_received;
|
|
|
|
while (*nmea_sentence != '\0' && *nmea_sentence != '*') {
|
|
checksum_computed ^= *nmea_sentence;
|
|
nmea_sentence++;
|
|
}
|
|
|
|
/* Make sure we're now pointing at the checksum */
|
|
if (*nmea_sentence == '\0') {
|
|
/* Buffer ran out before we found a checksum marker */
|
|
return false;
|
|
}
|
|
|
|
/* Load the checksum from the buffer */
|
|
checksum_received = strtol(nmea_sentence + 1, NULL, 16);
|
|
|
|
// PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed);
|
|
|
|
return checksum_computed == checksum_received;
|
|
}
|
|
|
|
/*
|
|
* This function only exists to deal with a linking
|
|
* failure in the stdlib function strtof(). This
|
|
* implementation does not rely on the _sbrk() syscall
|
|
* like strtof() does.
|
|
*/
|
|
|
|
/* Parse a number encoded in a string of the format:
|
|
* [-]NN.nnnnn
|
|
* into a signed whole part and an unsigned fractional part.
|
|
* The fract_units field indicates the units of the fractional part as
|
|
* 1 whole = 10^fract_units fract
|
|
*/
|
|
static bool NMEA_parse_real(int32_t *whole, uint32_t *fract, uint8_t *fract_units, char *field)
|
|
{
|
|
char *s = field;
|
|
char *field_w;
|
|
char *field_f;
|
|
|
|
PIOS_DEBUG_Assert(whole);
|
|
PIOS_DEBUG_Assert(fract);
|
|
PIOS_DEBUG_Assert(fract_units);
|
|
PIOS_DEBUG_Assert(field);
|
|
|
|
field_w = strsep(&s, ".");
|
|
field_f = s;
|
|
|
|
*whole = strtol(field_w, NULL, 10);
|
|
|
|
if (field_w) {
|
|
/* decimal was found so we may have a fractional part */
|
|
*fract = strtoul(field_f, NULL, 10);
|
|
*fract_units = strlen(field_f);
|
|
} else {
|
|
/* no decimal was found, fractional part is zero */
|
|
*fract = 0;
|
|
*fract_units = 0;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static float NMEA_real_to_float(char *nmea_real)
|
|
{
|
|
int32_t whole;
|
|
uint32_t fract;
|
|
uint8_t fract_units;
|
|
|
|
if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) {
|
|
return false;
|
|
}
|
|
|
|
/* Convert to float */
|
|
return ((float)whole) + fract * powf(10.0f, -fract_units);
|
|
}
|
|
|
|
/*
|
|
* Parse a field in the format:
|
|
* 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, bool negative)
|
|
{
|
|
int32_t num_DDDMM;
|
|
uint32_t num_m;
|
|
uint8_t units;
|
|
|
|
/* Sanity checks */
|
|
PIOS_DEBUG_Assert(nmea_latlon);
|
|
PIOS_DEBUG_Assert(latlon);
|
|
|
|
if (*nmea_latlon == '\0') { /* empty lat/lon field */
|
|
return false;
|
|
}
|
|
|
|
if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) {
|
|
return false;
|
|
}
|
|
|
|
/* scale up the mmmm[mm] field apropriately depending on # of digits */
|
|
/* not using 1eN notation because that forces fixed point and lost precision */
|
|
switch (units) {
|
|
case 0:
|
|
/* no digits, value is zero so no scaling */
|
|
break;
|
|
case 1: /* m */
|
|
num_m *= 1000000; /* m000000 */
|
|
break;
|
|
case 2: /* mm */
|
|
num_m *= 100000; /* mm00000 */
|
|
break;
|
|
case 3: /* mmm */
|
|
num_m *= 10000; /* mmm0000 */
|
|
break;
|
|
case 4: /* mmmm */
|
|
num_m *= 1000; /* mmmm000 */
|
|
break;
|
|
case 5: /* mmmmm */
|
|
num_m *= 100; /* mmmmm00 */
|
|
break;
|
|
case 6: /* mmmmmm */
|
|
num_m *= 10; /* mmmmmm0 */
|
|
break;
|
|
default:
|
|
/* unhandled format */
|
|
num_m = 0.0f;
|
|
break;
|
|
}
|
|
|
|
*latlon = (num_DDDMM / 100) * 10000000; /* scale the whole degrees */
|
|
*latlon += (num_DDDMM % 100) * 10000000 / 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;
|
|
}
|
|
|
|
|
|
/**
|
|
* Parses a complete NMEA sentence and updates the GPSPositionSensor UAVObject
|
|
* \param[in] An NMEA sentence with a valid checksum
|
|
* \return true if the sentence was successfully parsed
|
|
* \return false if any errors were encountered with the parsing
|
|
*/
|
|
bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
|
|
{
|
|
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
|
|
// Skip first two character, allow GL, GN, GP...
|
|
p += 2;
|
|
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
|
|
const struct nmea_parser *parser;
|
|
parser = NMEA_find_parser_by_prefix(params[0]);
|
|
if (!parser) {
|
|
// No parser found
|
|
#ifdef DEBUG_MSGID_IN
|
|
DEBUG_MSG(" NO PARSER (\"%s\")\n", params[0]);
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
#ifdef DEBUG_MSGID_IN
|
|
DEBUG_MSG("%s %d ", params[0]);
|
|
#endif
|
|
// Send the message to the parser and get it update the GpsData
|
|
// Information from various different NMEA messages are temporarily
|
|
// cumulated in the GpsData structure. An actual GPSPositionSensor update
|
|
// is triggered by GGA messages only. This message type sets the
|
|
// gpsDataUpdated flag to request this.
|
|
bool gpsDataUpdated = false;
|
|
|
|
if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
|
|
// Parse failed
|
|
#ifdef DEBUG_MSGID_IN
|
|
DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
|
|
#endif
|
|
if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
|
|
// leave my new field alone!
|
|
GPSPositionSensorBaudRateGet(&GpsData->BaudRate);
|
|
GPSPositionSensorSet(GpsData);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
// All is fine :) Update object if data has changed
|
|
if (gpsDataUpdated) {
|
|
#ifdef DEBUG_MSGID_IN
|
|
DEBUG_MSG("U");
|
|
#endif
|
|
// leave my new field alone!
|
|
GPSPositionSensorBaudRateGet(&GpsData->BaudRate);
|
|
GPSPositionSensorSet(GpsData);
|
|
}
|
|
|
|
#ifdef DEBUG_MSGID_IN
|
|
DEBUG_MSG("\n");
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
|
|
/**
|
|
* Parse an NMEA GxGGA sentence and update the given UAVObject
|
|
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
|
|
* \param[in] An NMEA sentence with a valid checksum
|
|
*/
|
|
static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
|
|
{
|
|
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
|
|
|
|
*gpsDataUpdated = true;
|
|
|
|
// check for invalid GPS fix
|
|
// do this first to make sure we get this information, even if later checks exit
|
|
// this function early
|
|
if (param[6][0] == '0') {
|
|
GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; // treat invalid fix as NOFIX
|
|
}
|
|
|
|
// get latitude [DDMM.mmmmm] [N|S]
|
|
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) {
|
|
return false;
|
|
}
|
|
|
|
// get longitude [dddmm.mmmmm] [E|W]
|
|
if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[4], param[5][0] == 'W')) {
|
|
return false;
|
|
}
|
|
|
|
// get number of satellites used in GPS solution
|
|
GpsData->Satellites = atoi(param[7]);
|
|
|
|
// get altitude (in meters mm.m)
|
|
GpsData->Altitude = NMEA_real_to_float(param[9]);
|
|
|
|
// geoid separation
|
|
GpsData->GeoidSeparation = NMEA_real_to_float(param[11]);
|
|
GpsData->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_NMEA;
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Parse an NMEA GxRMC sentence and update the given UAVObject
|
|
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
|
|
* \param[in] An NMEA sentence with a valid checksum
|
|
*/
|
|
static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
|
|
{
|
|
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 = false;
|
|
|
|
#if !defined(PIOS_GPS_MINIMAL)
|
|
GPSTimeData gpst;
|
|
GPSTimeGet(&gpst);
|
|
|
|
// 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;
|
|
#endif // PIOS_GPS_MINIMAL
|
|
|
|
// don't process void sentences
|
|
if (param[2][0] == 'V') {
|
|
return false;
|
|
}
|
|
|
|
// get latitude [DDMM.mmmmm] [N|S]
|
|
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) {
|
|
return false;
|
|
}
|
|
|
|
// get longitude [dddmm.mmmmm] [E|W]
|
|
if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[5], param[6][0] == 'W')) {
|
|
return false;
|
|
}
|
|
|
|
// get speed in knots
|
|
GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s
|
|
|
|
// get True course
|
|
GpsData->Heading = NMEA_real_to_float(param[8]);
|
|
|
|
#if !defined(PIOS_GPS_MINIMAL)
|
|
// get Date of fix
|
|
// TODO: Should really not use a float here to be safe
|
|
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;
|
|
GPSTimeSet(&gpst);
|
|
#endif // PIOS_GPS_MINIMAL
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Parse an NMEA GxVTG sentence and update the given UAVObject
|
|
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
|
|
* \param[in] An NMEA sentence with a valid checksum
|
|
*/
|
|
static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
|
|
{
|
|
if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) {
|
|
return false;
|
|
}
|
|
|
|
#ifdef NMEA_DEBUG_RMC
|
|
DEBUG_MSG("\n Heading=%s %s\n", param[1], param[2]);
|
|
DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]);
|
|
#endif
|
|
|
|
*gpsDataUpdated = false;
|
|
|
|
GpsData->Heading = NMEA_real_to_float(param[1]);
|
|
GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s
|
|
|
|
return true;
|
|
}
|
|
|
|
#if !defined(PIOS_GPS_MINIMAL)
|
|
/**
|
|
* Parse an NMEA GxZDA sentence and update the @ref GPSTime object
|
|
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused).
|
|
* \param[in] An NMEA sentence with a valid checksum
|
|
*/
|
|
static bool nmeaProcessGxZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
|
|
{
|
|
if (nbParam != 7) {
|
|
return false;
|
|
}
|
|
|
|
#ifdef NMEA_DEBUG_ZDA
|
|
DEBUG_MSG("\n Time=%s (hhmmss.ss)\n", param[1]);
|
|
DEBUG_MSG(" Date=%s/%s/%s (d/m/y)\n", param[2], param[3], param[4]);
|
|
#endif
|
|
|
|
*gpsDataUpdated = false; // Here we will never provide a new GPS value
|
|
|
|
// No new data data extracted
|
|
GPSTimeData gpst;
|
|
GPSTimeGet(&gpst);
|
|
|
|
// 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;
|
|
|
|
// Get Date
|
|
gpst.Day = atoi(param[2]);
|
|
gpst.Month = atoi(param[3]);
|
|
gpst.Year = atoi(param[4]);
|
|
|
|
GPSTimeSet(&gpst);
|
|
return true;
|
|
}
|
|
|
|
static GPSSatellitesData gsv_partial;
|
|
/* Bitmaps of which sentences we're looking for to allow us to handle out-of-order GSVs */
|
|
static uint8_t gsv_expected_mask;
|
|
static uint8_t gsv_processed_mask;
|
|
/* Error counters */
|
|
static uint16_t gsv_incomplete_error;
|
|
static uint16_t gsv_duplicate_error;
|
|
|
|
static bool nmeaProcessGxGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
|
|
{
|
|
if (nbParam < 4) {
|
|
return false;
|
|
}
|
|
|
|
#ifdef NMEA_DEBUG_GSV
|
|
DEBUG_MSG("\n Sentence=%s/%s\n", param[2], param[1]);
|
|
DEBUG_MSG(" Sats=%s\n", param[3]);
|
|
#endif
|
|
|
|
uint8_t nbSentences = atoi(param[1]);
|
|
uint8_t currSentence = atoi(param[2]);
|
|
|
|
*gpsDataUpdated = false;
|
|
|
|
if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) {
|
|
return false;
|
|
}
|
|
|
|
gsv_partial.SatsInView = atoi(param[3]);
|
|
|
|
// 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
|
|
gsv_incomplete_error++;
|
|
}
|
|
|
|
// First GSV sentence in the sequence, reset our expected_mask
|
|
gsv_expected_mask = (1 << nbSentences) - 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++;
|
|
} else {
|
|
/* Note that we've seen this 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 ((currSentence * 4) <= NELEMENTS(gsv_partial.PRN)) {
|
|
/* Process 4 blocks of satellite info */
|
|
for (uint8_t i = 0; parIdx + 4 <= nbParam && i < 4; i++) {
|
|
uint8_t sat_index = ((currSentence - 1) * 4) + i;
|
|
|
|
// Get sat info
|
|
gsv_partial.PRN[sat_index] = atoi(param[parIdx++]);
|
|
gsv_partial.Elevation[sat_index] = atoi(param[parIdx++]);
|
|
gsv_partial.Azimuth[sat_index] = atoi(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)) {
|
|
/* GSV set has been fully processed. Update the GPSSatellites object. */
|
|
GPSSatellitesSet(&gsv_partial);
|
|
memset((void *)&gsv_partial, 0, sizeof(gsv_partial));
|
|
gsv_expected_mask = 0;
|
|
gsv_processed_mask = 0;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
#endif // PIOS_GPS_MINIMAL
|
|
|
|
/**
|
|
* Parse an NMEA GPGSA sentence and update the given UAVObject
|
|
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
|
|
* \param[in] An NMEA sentence with a valid checksum
|
|
*/
|
|
static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
|
|
{
|
|
if (nbParam != 18) {
|
|
return false;
|
|
}
|
|
|
|
#ifdef NMEA_DEBUG_GSA
|
|
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
|
|
|
|
*gpsDataUpdated = false;
|
|
|
|
switch (atoi(param[2])) {
|
|
case 1:
|
|
GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
|
|
break;
|
|
case 2:
|
|
GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX2D;
|
|
break;
|
|
case 3:
|
|
GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX3D;
|
|
break;
|
|
default:
|
|
/* Unhandled */
|
|
return false;
|
|
|
|
break;
|
|
}
|
|
|
|
// next field: PDOP
|
|
GpsData->PDOP = NMEA_real_to_float(param[15]);
|
|
|
|
// next field: HDOP
|
|
GpsData->HDOP = NMEA_real_to_float(param[16]);
|
|
|
|
// next field: VDOP
|
|
GpsData->VDOP = NMEA_real_to_float(param[17]);
|
|
|
|
return true;
|
|
}
|
|
|
|
#endif // PIOS_INCLUDE_GPS_NMEA_PARSER
|