flight/modules/GPS/NMEA.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup GPSModule GPS Module
   6  * @brief Process GPS information (NMEA format)
   7  * @{
   8  *
   9  * @file       NMEA.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  12  * @brief      GPS module, handles GPS and NMEA stream
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "openpilot.h"
  33 #include "pios.h"
  34
  35 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
  36
  37 #include "gpspositionsensor.h"
  38 #include "NMEA.h"
  39 #include "gpstime.h"
  40 #include "gpssatellites.h"
  41 #include "GPS.h"
  42
  43 // #define ENABLE_DEBUG_MSG                                             ///< define to enable debug-messages
  44 #define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is used for debug-messages
  45
  46 // Debugging
  47 #ifdef ENABLE_DEBUG_MSG
  48 // #define DEBUG_MSG_IN                 ///< define to display the incoming NMEA messages
  49 // #define DEBUG_PARAMS                 ///< define to display the incoming NMEA messages split into its parameters
  50 // #define DEBUG_MSGID_IN               ///< define to display the names of the incoming NMEA messages
  51 // #define NMEA_DEBUG_PKT               ///< define to enable debug of all NMEA messages
  52 // #define NMEA_DEBUG_GGA               ///< define to enable debug of GGA messages
  53 // #define NMEA_DEBUG_VTG               ///< define to enable debug of VTG messages
  54 // #define NMEA_DEBUG_RMC               ///< define to enable debug of RMC messages
  55 // #define NMEA_DEBUG_GSA               ///< define to enable debug of GSA messages
  56 // #define NMEA_DEBUG_GSV               ///< define to enable debug of GSV messages
  57 // #define NMEA_DEBUG_ZDA               ///< define to enable debug of ZDA messages
  58 #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__)
  59 #else
  60 #define DEBUG_MSG(format, ...)
  61 #endif
  62
  63 #define MAX_NB_PARAMS 20
  64 /* NMEA sentence parsers */
  65
  66 struct nmea_parser {
  67     const char *prefix;
  68     bool (*handler)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
  69 };
  70
  71 static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
  72 static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
  73 static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
  74 static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
  75 #if !defined(PIOS_GPS_MINIMAL)
  76 static bool nmeaProcessGxZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
  77 static bool nmeaProcessGxGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
  78 #endif // PIOS_GPS_MINIMAL
  79
  80 static const struct nmea_parser nmea_parsers[] = {
  81     {
  82         .prefix  = "GGA",
  83         .handler = nmeaProcessGxGGA,
  84     },
  85     {
  86         .prefix  = "VTG",
  87         .handler = nmeaProcessGxVTG,
  88     },
  89     {
  90         .prefix  = "GSA",
  91         .handler = nmeaProcessGxGSA,
  92     },
  93     {
  94         .prefix  = "RMC",
  95         .handler = nmeaProcessGxRMC,
  96     },
  97 #if !defined(PIOS_GPS_MINIMAL)
  98     {
  99         .prefix  = "ZDA",
 100         .handler = nmeaProcessGxZDA,
 101     },
 102     {
 103         .prefix  = "GSV",
 104         .handler = nmeaProcessGxGSV,
 105     },
 106 #endif // PIOS_GPS_MINIMAL
 107 };
 108
 109 int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
 110 {
 111     static uint8_t rx_count = 0;
 112     static bool start_flag  = false;
 113     static bool found_cr    = false;
 114     bool goodParse = false;
 115     uint8_t c;
 116     int i = 0;
 117
 118     while (i < len) {
 119         c = rx[i++];
 120         // detect start while acquiring stream
 121         // if we find a $ in the middle it was a bad packet (e.g. maybe UBX binary),
 122         // and this may be the start of another packet
 123         // silently cancel the current sentence
 124         if (c == '$') { // NMEA identifier found
 125             start_flag = false;
 126         }
 127         if (!start_flag) { // if no NMEA identifier ('$') found yet
 128             if (c == '$') { // NMEA identifier found
 129                 start_flag = true;
 130                 found_cr   = false;
 131                 rx_count   = 0;
 132             } else {
 133                 // find a likely candidate for a NMEA string
 134                 // skip over some e.g. uBlox packets
 135                 uint8_t *p;
 136                 p = memchr(&rx[i], '$', len - i);
 137                 if (p) {
 138                     i += p - &rx[i];
 139                 } else {
 140                     i = len;
 141                 }
 142                 // loop to restart at the $ if there is one
 143                 continue;
 144             }
 145         }
 146         if (rx_count >= NMEA_MAX_PACKET_LENGTH) {
 147             // The buffer is already full and we haven't found a valid NMEA sentence.
 148             // Flush the buffer and note the overflow event.
 149             gpsRxStats->gpsRxOverflow++;
 150             start_flag = false;
 151             continue;
 152         } else {
 153             gps_rx_buffer[rx_count++] = c;
 154         }
 155
 156         // look for ending '\r\n' sequence
 157         if (!found_cr && (c == '\r')) {
 158             found_cr = true;
 159         } else if (found_cr) {
 160             if (c != '\n') {
 161                 found_cr = false; // false end flag
 162             } else {
 163                 // The NMEA functions require a zero-terminated string
 164                 // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
 165                 gps_rx_buffer[rx_count - 2] = 0;
 166
 167                 // prepare to parse next sentence
 168                 start_flag = false;
 169                 // Our rxBuffer must look like this now:
 170                 // [0]           = '$'
 171                 // ...           = zero or more bytes of sentence payload
 172                 // [end_pos - 1] = '\r'
 173                 // [end_pos]     = '\n'
 174                 //
 175                 // Prepare to consume the sentence from the buffer
 176
 177                 // Validate the checksum over the sentence
 178                 if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum.  May indicate dropped characters on Rx.
 179                     // PIOS_DEBUG_PinHigh(2);
 180                     gpsRxStats->gpsRxChkSumError++;
 181                     // PIOS_DEBUG_PinLow(2);
 182                 } else { // Valid checksum, use this packet to update the GPS position
 183                     if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
 184                         // PIOS_DEBUG_PinHigh(2);
 185                         gpsRxStats->gpsRxParserError++;
 186                         // PIOS_DEBUG_PinLow(2);
 187                     } else {
 188                         gpsRxStats->gpsRxReceived++;
 189                         goodParse = true;
 190                     }
 191                 }
 192                 continue;
 193             }
 194         }
 195     }
 196
 197     if (goodParse) {
 198         // if so much as one good sentence we return a good status so the connection status says "alive"
 199         // if we didn't do this, a lot of garbage (e.g. UBX protocol) mixed in with enough NMEA to fly
 200         // might think the GPS was offline
 201         return PARSER_COMPLETE;
 202     } else {
 203         return PARSER_INCOMPLETE;
 204     }
 205 }
 206
 207 static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
 208 {
 209     if (!prefix) {
 210         return NULL;
 211     }
 212
 213     for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) {
 214         const struct nmea_parser *parser = &nmea_parsers[i];
 215
 216         /* Use strcmp to check for exact equality over the entire prefix */
 217         if (!strcmp(prefix, parser->prefix)) {
 218             /* Found an appropriate parser */
 219             return parser;
 220         }
 221     }
 222
 223     /* No matching parser for this prefix */
 224     return NULL;
 225 }
 226
 227 /**
 228  * Computes NMEA sentence checksum
 229  * \param[in] Buffer for parsed nmea sentence
 230  * \return false checksum not valid
 231  * \return true checksum valid
 232  */
 233 bool NMEA_checksum(char *nmea_sentence)
 234 {
 235     uint8_t checksum_computed = 0;
 236     uint8_t checksum_received;
 237
 238     while (*nmea_sentence != '\0' && *nmea_sentence != '*') {
 239         checksum_computed ^= *nmea_sentence;
 240         nmea_sentence++;
 241     }
 242
 243     /* Make sure we're now pointing at the checksum */
 244     if (*nmea_sentence == '\0') {
 245         /* Buffer ran out before we found a checksum marker */
 246         return false;
 247     }
 248
 249     /* Load the checksum from the buffer */
 250     checksum_received = strtol(nmea_sentence + 1, NULL, 16);
 251
 252     // PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed);
 253
 254     return checksum_computed == checksum_received;
 255 }
 256
 257 /*
 258  * This function only exists to deal with a linking
 259  * failure in the stdlib function strtof().  This
 260  * implementation does not rely on the _sbrk() syscall
 261  * like strtof() does.
 262  */
 263
 264 /* Parse a number encoded in a string of the format:
 265  *   [-]NN.nnnnn
 266  * into a signed whole part and an unsigned fractional part.
 267  * The fract_units field indicates the units of the fractional part as
 268  *   1 whole = 10^fract_units fract
 269  */
 270 static bool NMEA_parse_real(int32_t *whole, uint32_t *fract, uint8_t *fract_units, char *field)
 271 {
 272     char *s = field;
 273     char *field_w;
 274     char *field_f;
 275
 276     PIOS_DEBUG_Assert(whole);
 277     PIOS_DEBUG_Assert(fract);
 278     PIOS_DEBUG_Assert(fract_units);
 279     PIOS_DEBUG_Assert(field);
 280
 281     field_w = strsep(&s, ".");
 282     field_f = s;
 283
 284     *whole  = strtol(field_w, NULL, 10);
 285
 286     if (field_w) {
 287         /* decimal was found so we may have a fractional part */
 288         *fract = strtoul(field_f, NULL, 10);
 289         *fract_units = strlen(field_f);
 290     } else {
 291         /* no decimal was found, fractional part is zero */
 292         *fract = 0;
 293         *fract_units = 0;
 294     }
 295
 296     return true;
 297 }
 298
 299 static float NMEA_real_to_float(char *nmea_real)
 300 {
 301     int32_t whole;
 302     uint32_t fract;
 303     uint8_t fract_units;
 304
 305     if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) {
 306         return false;
 307     }
 308
 309     /* Convert to float */
 310     return ((float)whole) + fract * powf(10.0f, -fract_units);
 311 }
 312
 313 /*
 314  * Parse a field in the format:
 315  *    DD[D]MM.mmmm[mm]
 316  * into a fixed-point representation in units of (degrees * 1e-7)
 317  */
 318 static bool NMEA_latlon_to_fixed_point(int32_t *latlon, char *nmea_latlon, bool negative)
 319 {
 320     int32_t num_DDDMM;
 321     uint32_t num_m;
 322     uint8_t units;
 323
 324     /* Sanity checks */
 325     PIOS_DEBUG_Assert(nmea_latlon);
 326     PIOS_DEBUG_Assert(latlon);
 327
 328     if (*nmea_latlon == '\0') { /* empty lat/lon field */
 329         return false;
 330     }
 331
 332     if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) {
 333         return false;
 334     }
 335
 336     /* scale up the mmmm[mm] field apropriately depending on # of digits */
 337     /* not using 1eN notation because that forces fixed point and lost precision */
 338     switch (units) {
 339     case 0:
 340         /* no digits, value is zero so no scaling */
 341         break;
 342     case 1: /* m       */
 343         num_m *= 1000000; /* m000000 */
 344         break;
 345     case 2: /* mm      */
 346         num_m *= 100000; /* mm00000 */
 347         break;
 348     case 3: /* mmm     */
 349         num_m *= 10000; /* mmm0000 */
 350         break;
 351     case 4: /* mmmm    */
 352         num_m *= 1000; /* mmmm000 */
 353         break;
 354     case 5: /* mmmmm   */
 355         num_m *= 100; /* mmmmm00 */
 356         break;
 357     case 6: /* mmmmmm  */
 358         num_m *= 10; /* mmmmmm0 */
 359         break;
 360     default:
 361         /* unhandled format */
 362         num_m = 0.0f;
 363         break;
 364     }
 365
 366     *latlon  = (num_DDDMM / 100) * 10000000;        /* scale the whole degrees */
 367     *latlon += (num_DDDMM % 100) * 10000000 / 60; /* add in the scaled decimal whole minutes */
 368     *latlon += num_m / 60; /* add in the scaled decimal fractional minutes */
 369
 370     if (negative) {
 371         *latlon *= -1;
 372     }
 373
 374     return true;
 375 }
 376
 377
 378 /**
 379  * Parses a complete NMEA sentence and updates the GPSPositionSensor UAVObject
 380  * \param[in] An NMEA sentence with a valid checksum
 381  * \return true if the sentence was successfully parsed
 382  * \return false if any errors were encountered with the parsing
 383  */
 384 bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
 385 {
 386     char *p = nmea_sentence;
 387     char *params[MAX_NB_PARAMS];
 388     uint8_t nbParams;
 389
 390 #ifdef DEBUG_MSG_IN
 391     DEBUG_MSG("\"%s\"\n", nmea_sentence);
 392 #endif
 393
 394     // Split the nmea sentence it its parameters, separated by ","
 395     // Sample NMEA message: "GPRMC,000131.736,V,,,,,0.00,0.00,060180,,,N*43"
 396
 397     // The first parameter starts at the beginning of the message
 398     // Skip first two character, allow GL, GN, GP...
 399     p += 2;
 400     params[0] = p;
 401     nbParams  = 1;
 402     while (*p != 0) {
 403         if (*p == '*') {
 404             // After the * comes the "CRC", we are done,
 405             *p = 0; // Zero-terminate this parameter
 406             break;
 407         } else if (*p == ',') {
 408             // This is the end of this parameter
 409             *p = 0; // Zero-terminate this parameter
 410             // Start new parameter
 411             if (nbParams == MAX_NB_PARAMS) {
 412                 break;
 413             }
 414             params[nbParams] = p + 1; // For sure there is something at p+1 because at p there is ","
 415             nbParams++;
 416         }
 417         p++;
 418     }
 419
 420
 421 #ifdef DEBUG_PARAMS
 422     int i;
 423     for (i = 0; i < nbParams; i++) {
 424         DEBUG_MSG(" %d \"%s\"\n", i, params[i]);
 425     }
 426 #endif
 427
 428     // The first parameter is the message name, lets see if we find a parser for it
 429     const struct nmea_parser *parser;
 430     parser = NMEA_find_parser_by_prefix(params[0]);
 431     if (!parser) {
 432         // No parser found
 433                 #ifdef DEBUG_MSGID_IN
 434         DEBUG_MSG(" NO PARSER (\"%s\")\n", params[0]);
 435                 #endif
 436         return false;
 437     }
 438
 439         #ifdef DEBUG_MSGID_IN
 440     DEBUG_MSG("%s %d ", params[0]);
 441         #endif
 442     // Send the message to the parser and get it update the GpsData
 443     // Information from various different NMEA messages are temporarily
 444     // cumulated in the GpsData structure. An actual GPSPositionSensor update
 445     // is triggered by GGA messages only. This message type sets the
 446     // gpsDataUpdated flag to request this.
 447     bool gpsDataUpdated = false;
 448
 449     if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
 450         // Parse failed
 451                 #ifdef DEBUG_MSGID_IN
 452         DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
 453                 #endif
 454         if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
 455             // leave my new field alone!
 456             GPSPositionSensorBaudRateGet(&GpsData->BaudRate);
 457             GPSPositionSensorSet(GpsData);
 458         }
 459         return false;
 460     }
 461
 462
 463     // All is fine :)  Update object if data has changed
 464     if (gpsDataUpdated) {
 465                 #ifdef DEBUG_MSGID_IN
 466         DEBUG_MSG("U");
 467                 #endif
 468         // leave my new field alone!
 469         GPSPositionSensorBaudRateGet(&GpsData->BaudRate);
 470         GPSPositionSensorSet(GpsData);
 471     }
 472
 473         #ifdef DEBUG_MSGID_IN
 474     DEBUG_MSG("\n");
 475         #endif
 476     return true;
 477 }
 478
 479
 480 /**
 481  * Parse an NMEA GxGGA sentence and update the given UAVObject
 482  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
 483  * \param[in] An NMEA sentence with a valid checksum
 484  */
 485 static bool nmeaProcessGxGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 486 {
 487     if (nbParam != 15) {
 488         return false;
 489     }
 490
 491 #ifdef  NMEA_DEBUG_GGA
 492     DEBUG_MSG("\n UTC=%s\n", param[1]);
 493     DEBUG_MSG(" Lat=%s %s\n", param[2], param[3]);
 494     DEBUG_MSG(" Long=%s %s\n", param[4], param[5]);
 495     DEBUG_MSG(" Fix=%s\n", param[6]);
 496     DEBUG_MSG(" Sat=%s\n", param[7]);
 497     DEBUG_MSG(" HDOP=%s\n", param[8]);
 498     DEBUG_MSG(" Alt=%s %s\n", param[9], param[10]);
 499     DEBUG_MSG(" GeoidSep=%s %s\n\n", param[11]);
 500 #endif
 501
 502     *gpsDataUpdated = true;
 503
 504     // check for invalid GPS fix
 505     // do this first to make sure we get this information, even if later checks exit
 506     // this function early
 507     if (param[6][0] == '0') {
 508         GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; // treat invalid fix as NOFIX
 509     }
 510
 511     // get latitude [DDMM.mmmmm] [N|S]
 512     if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) {
 513         return false;
 514     }
 515
 516     // get longitude [dddmm.mmmmm] [E|W]
 517     if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[4], param[5][0] == 'W')) {
 518         return false;
 519     }
 520
 521     // get number of satellites used in GPS solution
 522     GpsData->Satellites = atoi(param[7]);
 523
 524     // get altitude (in meters mm.m)
 525     GpsData->Altitude   = NMEA_real_to_float(param[9]);
 526
 527     // geoid separation
 528     GpsData->GeoidSeparation = NMEA_real_to_float(param[11]);
 529     GpsData->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_NMEA;
 530     return true;
 531 }
 532
 533 /**
 534  * Parse an NMEA GxRMC sentence and update the given UAVObject
 535  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
 536  * \param[in] An NMEA sentence with a valid checksum
 537  */
 538 static bool nmeaProcessGxRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 539 {
 540     if (nbParam != 13) {
 541         return false;
 542     }
 543
 544 #ifdef NMEA_DEBUG_RMC
 545     DEBUG_MSG("\n UTC=%s\n", param[1]);
 546     DEBUG_MSG(" Lat=%s %s\n", param[3], param[4]);
 547     DEBUG_MSG(" Long=%s %s\n", param[5], param[6]);
 548     DEBUG_MSG(" Speed=%s\n", param[7]);
 549     DEBUG_MSG(" Course=%s\n", param[8]);
 550     DEBUG_MSG(" DateOfFix=%s\n\n", param[9]);
 551 #endif
 552
 553     *gpsDataUpdated = false;
 554
 555 #if !defined(PIOS_GPS_MINIMAL)
 556     GPSTimeData gpst;
 557     GPSTimeGet(&gpst);
 558
 559     // get UTC time [hhmmss.sss]
 560     float hms = NMEA_real_to_float(param[1]);
 561     gpst.Second = (int)hms % 100;
 562     gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
 563     gpst.Hour   = (int)hms / 10000;
 564 #endif // PIOS_GPS_MINIMAL
 565
 566     // don't process void sentences
 567     if (param[2][0] == 'V') {
 568         return false;
 569     }
 570
 571     // get latitude [DDMM.mmmmm] [N|S]
 572     if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) {
 573         return false;
 574     }
 575
 576     // get longitude [dddmm.mmmmm] [E|W]
 577     if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, param[5], param[6][0] == 'W')) {
 578         return false;
 579     }
 580
 581     // get speed in knots
 582     GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s
 583
 584     // get True course
 585     GpsData->Heading     = NMEA_real_to_float(param[8]);
 586
 587 #if !defined(PIOS_GPS_MINIMAL)
 588     // get Date of fix
 589     // TODO: Should really not use a float here to be safe
 590     float date = NMEA_real_to_float(param[9]);
 591     gpst.Year  = (int)date % 100;
 592     gpst.Month = (((int)date - gpst.Year) / 100) % 100;
 593     gpst.Day   = (int)(date / 10000);
 594     gpst.Year += 2000;
 595     GPSTimeSet(&gpst);
 596 #endif // PIOS_GPS_MINIMAL
 597
 598     return true;
 599 }
 600
 601 /**
 602  * Parse an NMEA GxVTG sentence and update the given UAVObject
 603  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
 604  * \param[in] An NMEA sentence with a valid checksum
 605  */
 606 static bool nmeaProcessGxVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 607 {
 608     if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) {
 609         return false;
 610     }
 611
 612 #ifdef NMEA_DEBUG_RMC
 613     DEBUG_MSG("\n Heading=%s %s\n", param[1], param[2]);
 614     DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]);
 615 #endif
 616
 617     *gpsDataUpdated      = false;
 618
 619     GpsData->Heading     = NMEA_real_to_float(param[1]);
 620     GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s
 621
 622     return true;
 623 }
 624
 625 #if !defined(PIOS_GPS_MINIMAL)
 626 /**
 627  * Parse an NMEA GxZDA sentence and update the @ref GPSTime object
 628  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused).
 629  * \param[in] An NMEA sentence with a valid checksum
 630  */
 631 static bool nmeaProcessGxZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 632 {
 633     if (nbParam != 7) {
 634         return false;
 635     }
 636
 637         #ifdef NMEA_DEBUG_ZDA
 638     DEBUG_MSG("\n Time=%s (hhmmss.ss)\n", param[1]);
 639     DEBUG_MSG(" Date=%s/%s/%s (d/m/y)\n", param[2], param[3], param[4]);
 640         #endif
 641
 642     *gpsDataUpdated = false; // Here we will never provide a new GPS value
 643
 644     // No new data data extracted
 645     GPSTimeData gpst;
 646     GPSTimeGet(&gpst);
 647
 648     // get UTC time [hhmmss.sss]
 649     float hms = NMEA_real_to_float(param[1]);
 650     gpst.Second = (int)hms % 100;
 651     gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
 652     gpst.Hour   = (int)hms / 10000;
 653
 654     // Get Date
 655     gpst.Day    = atoi(param[2]);
 656     gpst.Month  = atoi(param[3]);
 657     gpst.Year   = atoi(param[4]);
 658
 659     GPSTimeSet(&gpst);
 660     return true;
 661 }
 662
 663 static GPSSatellitesData gsv_partial;
 664 /* Bitmaps of which sentences we're looking for to allow us to handle out-of-order GSVs */
 665 static uint8_t gsv_expected_mask;
 666 static uint8_t gsv_processed_mask;
 667 /* Error counters */
 668 static uint16_t gsv_incomplete_error;
 669 static uint16_t gsv_duplicate_error;
 670
 671 static bool nmeaProcessGxGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 672 {
 673     if (nbParam < 4) {
 674         return false;
 675     }
 676
 677 #ifdef NMEA_DEBUG_GSV
 678     DEBUG_MSG("\n Sentence=%s/%s\n", param[2], param[1]);
 679     DEBUG_MSG(" Sats=%s\n", param[3]);
 680 #endif
 681
 682     uint8_t nbSentences  = atoi(param[1]);
 683     uint8_t currSentence = atoi(param[2]);
 684
 685     *gpsDataUpdated = false;
 686
 687     if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) {
 688         return false;
 689     }
 690
 691     gsv_partial.SatsInView = atoi(param[3]);
 692
 693     // Find out if this is the first sentence in the GSV set
 694     if (currSentence == 1) {
 695         if (gsv_expected_mask != gsv_processed_mask) {
 696             // We are starting over when we haven't yet finished our previous GSV group
 697             gsv_incomplete_error++;
 698         }
 699
 700         // First GSV sentence in the sequence, reset our expected_mask
 701         gsv_expected_mask = (1 << nbSentences) - 1;
 702     }
 703
 704     uint8_t current_sentence_id = (1 << (currSentence - 1));
 705     if (gsv_processed_mask & current_sentence_id) {
 706         /* Duplicate sentence in this GSV set */
 707         gsv_duplicate_error++;
 708     } else {
 709         /* Note that we've seen this sentence */
 710         gsv_processed_mask |= current_sentence_id;
 711     }
 712
 713     uint8_t parIdx = 4;
 714
 715 #ifdef NMEA_DEBUG_GSV
 716     DEBUG_MSG(" PRN:");
 717 #endif
 718
 719     /* Make sure this sentence can fit in our GPSSatellites object */
 720     if ((currSentence * 4) <= NELEMENTS(gsv_partial.PRN)) {
 721         /* Process 4 blocks of satellite info */
 722         for (uint8_t i = 0; parIdx + 4 <= nbParam && i < 4; i++) {
 723             uint8_t sat_index = ((currSentence - 1) * 4) + i;
 724
 725             // Get sat info
 726             gsv_partial.PRN[sat_index]       = atoi(param[parIdx++]);
 727             gsv_partial.Elevation[sat_index] = atoi(param[parIdx++]);
 728             gsv_partial.Azimuth[sat_index]   = atoi(param[parIdx++]);
 729             gsv_partial.SNR[sat_index]       = atoi(param[parIdx++]);
 730 #ifdef NMEA_DEBUG_GSV
 731             DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]);
 732 #endif
 733         }
 734     }
 735 #ifdef NMEA_DEBUG_GSV
 736     DEBUG_MSG("\n");
 737 #endif
 738
 739
 740     /* Find out if we're finished processing all GSV sentences in the set */
 741     if ((gsv_expected_mask != 0) && (gsv_processed_mask == gsv_expected_mask)) {
 742         /* GSV set has been fully processed.  Update the GPSSatellites object. */
 743         GPSSatellitesSet(&gsv_partial);
 744         memset((void *)&gsv_partial, 0, sizeof(gsv_partial));
 745         gsv_expected_mask  = 0;
 746         gsv_processed_mask = 0;
 747     }
 748
 749     return true;
 750 }
 751 #endif // PIOS_GPS_MINIMAL
 752
 753 /**
 754  * Parse an NMEA GPGSA sentence and update the given UAVObject
 755  * \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
 756  * \param[in] An NMEA sentence with a valid checksum
 757  */
 758 static bool nmeaProcessGxGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
 759 {
 760     if (nbParam != 18) {
 761         return false;
 762     }
 763
 764 #ifdef NMEA_DEBUG_GSA
 765     DEBUG_MSG("\n Status=%s\n", param[2]);
 766     DEBUG_MSG(" PDOP=%s\n", param[15]);
 767     DEBUG_MSG(" HDOP=%s\n", param[16]);
 768     DEBUG_MSG(" VDOP=%s\n", param[17]);
 769 #endif
 770
 771     *gpsDataUpdated = false;
 772
 773     switch (atoi(param[2])) {
 774     case 1:
 775         GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
 776         break;
 777     case 2:
 778         GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX2D;
 779         break;
 780     case 3:
 781         GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX3D;
 782         break;
 783     default:
 784         /* Unhandled */
 785         return false;
 786
 787         break;
 788     }
 789
 790     // next field: PDOP
 791     GpsData->PDOP = NMEA_real_to_float(param[15]);
 792
 793     // next field: HDOP
 794     GpsData->HDOP = NMEA_real_to_float(param[16]);
 795
 796     // next field: VDOP
 797     GpsData->VDOP = NMEA_real_to_float(param[17]);
 798
 799     return true;
 800 }
 801
 802 #endif // PIOS_INCLUDE_GPS_NMEA_PARSER