flight/modules/GPS/NMEA.c

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