ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       nmeaparser.cpp
   5  * @author     Sami Korhonen Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup GPSGadgetPlugin GPS Gadget Plugin
   9  * @{
  10  * @brief A gadget that displays GPS status and enables basic configuration
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28
  29 #include "nmeaparser.h"
  30 #include <iostream>
  31 #include <math.h>
  32 #include <QDebug>
  33 #include <QStringList>
  34 #include <QWidget>
  35 #include <QVBoxLayout>
  36 #include <QPushButton>
  37
  38 // Message Codes
  39 #define NMEA_NODATA    0       // No data. Packet not available, bad, or not decoded
  40 #define NMEA_GPGGA     1       // Global Positioning System Fix Data
  41 #define NMEA_GPVTG     2       // Course over ground and ground speed
  42 #define NMEA_GPGLL     3       // Geographic position - latitude/longitude
  43 #define NMEA_GPGSV     4       // GPS satellites in view
  44 #define NMEA_GPGSA     5       // GPS DOP and active satellites
  45 #define NMEA_GPRMC     6       // Recommended minimum specific GPS data
  46 #define NMEA_GPZDA     7       // Time and Date
  47 #define NMEA_UNKNOWN   0xFF // Packet received but not known
  48
  49 #define GPS_TIMEOUT_MS 500
  50
  51 // Debugging
  52
  53 // #define GPSDEBUG
  54 // #define NMEA_DEBUG_PKT       ///< define to enable debug of all NMEA messages
  55
  56 #ifdef GPSDEBUG
  57         #define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
  58         #define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
  59         #define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages
  60         #define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages
  61         #define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
  62         #define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages
  63 #endif
  64
  65 /**
  66  * Initialize the parser
  67  */
  68 NMEAParser::NMEAParser(QObject *parent) : GPSParser(parent)
  69 {
  70     bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
  71     gpsRxOverflow = 0;
  72 }
  73
  74 NMEAParser::~NMEAParser()
  75 {}
  76
  77 /**
  78  * Called each time there are data in the input buffer
  79  */
  80 void NMEAParser::processInputStream(char c)
  81 {
  82     if (!bufferAddToEnd(&gpsRxBuffer, c)) {
  83         // no space in buffer
  84         // count overflow
  85         gpsRxOverflow++;
  86         return;
  87     }
  88     nmeaProcess(&gpsRxBuffer);
  89 }
  90
  91
  92 /**
  93  * Prosesses NMEA sentence checksum
  94  * \param[in] Buffer for parsed nmea sentence
  95  * \return 0 checksum not valid
  96  * \return 1 checksum valid
  97  */
  98 char NMEAParser::nmeaChecksum(char *gps_buffer)
  99 {
 100     char checksum = 0;
 101     char checksum_received = 0;
 102
 103     for (int x = 0; x < NMEA_BUFFERSIZE; x++) {
 104         if (gps_buffer[x] == '*') {
 105             // Parsing received checksum...
 106             checksum_received = strtol(&gps_buffer[x + 1], NULL, 16);
 107
 108             break;
 109         } else {
 110             // XOR the received data...
 111             checksum ^= gps_buffer[x];
 112         }
 113     }
 114     if (checksum == checksum_received) {
 115         ++numUpdates;
 116         return 1;
 117     } else {
 118         ++numErrors;
 119         return 0;
 120     }
 121 }
 122
 123 void NMEAParser::nmeaTerminateAtChecksum(char *gps_buffer)
 124 {
 125     for (int x = 0; x < NMEA_BUFFERSIZE; x++) {
 126         if (gps_buffer[x] == '*') {
 127             gps_buffer[x] = 0;
 128             break;
 129         }
 130     }
 131 }
 132
 133 /**
 134  * Prosesses NMEA sentences
 135  * \param[in] cBuffer for prosessed nmea sentences
 136  * \return Message code for found packet
 137  * \return 0xFF NO packet found
 138  */
 139 uint8_t NMEAParser::nmeaProcess(cBuffer *rxBuffer)
 140 {
 141     uint8_t foundpacket = NMEA_NODATA;
 142     uint8_t startFlag   = false;
 143     // u08 data;
 144     uint16_t i, j;
 145
 146     // process the receive buffer
 147     // go through buffer looking for packets
 148     while (rxBuffer->datalength) {
 149         // look for a start of NMEA packet
 150         if (bufferGetAtIndex(rxBuffer, 0) == '$') {
 151             // found start
 152             startFlag = true;
 153             // when start is found, we leave it intact in the receive buffer
 154             // in case the full NMEA string is not completely received.  The
 155             // start will be detected in the next nmeaProcess iteration.
 156
 157             // done looking for start
 158             break;
 159         } else {
 160             bufferGetFromFront(rxBuffer);
 161         }
 162     }
 163
 164     // if we detected a start, look for end of packet
 165     if (startFlag) {
 166         for (i = 1; i < (rxBuffer->datalength) - 1; i++) {
 167             // check for end of NMEA packet <CR><LF>
 168             if ((bufferGetAtIndex(rxBuffer, i) == '\r') && (bufferGetAtIndex(rxBuffer, i + 1) == '\n')) {
 169                 // have a packet end
 170                 // dump initial '$'
 171                 bufferGetFromFront(rxBuffer);
 172                 // copy packet to NmeaPacket
 173                 for (j = 0; j < (i - 1); j++) {
 174                     // although NMEA strings should be 80 characters or less,
 175                     // receive buffer errors can generate erroneous packets.
 176                     // Protect against packet buffer overflow
 177                     if (j < (NMEA_BUFFERSIZE - 1)) {
 178                         NmeaPacket[j] = bufferGetFromFront(rxBuffer);
 179                     } else {
 180                         bufferGetFromFront(rxBuffer);
 181                     }
 182                 }
 183                 // null terminate it
 184                 if (j < (NMEA_BUFFERSIZE - 1)) {
 185                     NmeaPacket[j] = 0;
 186                 } else {
 187                     NmeaPacket[NMEA_BUFFERSIZE - 1] = 0;
 188                 }
 189                 // dump <CR><LF> from rxBuffer
 190                 bufferGetFromFront(rxBuffer);
 191                 bufferGetFromFront(rxBuffer);
 192                 // DEBUG
 193                                 #ifdef NMEA_DEBUG_PKT
 194                 qDebug() << NmeaPacket;
 195                                 #endif
 196                 emit packet(QString(NmeaPacket));
 197                 // found a packet
 198                 // done with this processing session
 199                 foundpacket = NMEA_UNKNOWN;
 200                 break;
 201             }
 202         }
 203     }
 204
 205     if (foundpacket) {
 206         // check message type and process appropriately
 207         if (!strncmp(NmeaPacket, "GPGGA", 5)) {
 208             // process packet of this type
 209             nmeaProcessGPGGA(NmeaPacket);
 210             // report packet type
 211             foundpacket = NMEA_GPGGA;
 212         } else if (!strncmp(NmeaPacket, "GPVTG", 5)) {
 213             // process packet of this type
 214             nmeaProcessGPVTG(NmeaPacket);
 215             // report packet type
 216             foundpacket = NMEA_GPVTG;
 217         } else if (!strncmp(NmeaPacket, "GPGSA", 5)) {
 218             // process packet of this type
 219             nmeaProcessGPGSA(NmeaPacket);
 220             // report packet type
 221             foundpacket = NMEA_GPGSA;
 222         } else if (!strncmp(NmeaPacket, "GPRMC", 5)) {
 223             // process packet of this type
 224             nmeaProcessGPRMC(NmeaPacket);
 225             // report packet type
 226             foundpacket = NMEA_GPRMC;
 227         } else if (!strncmp(NmeaPacket, "GPGSV", 5)) {
 228             // Process packet of this type
 229             nmeaProcessGPGSV(NmeaPacket);
 230             // rerpot packet type
 231             foundpacket = NMEA_GPGSV;
 232         } else if (!strncmp(NmeaPacket, "GPZDA", 5)) {
 233             // Process packet of this type
 234             nmeaProcessGPZDA(NmeaPacket);
 235             // rerpot packet type
 236             foundpacket = NMEA_GPZDA;
 237         }
 238     } else if (rxBuffer->datalength >= rxBuffer->size) {
 239         // if we found no packet, and the buffer is full
 240         // we're logjammed, flush entire buffer
 241         bufferFlush(rxBuffer);
 242     }
 243     return foundpacket;
 244 }
 245
 246 /**
 247  * Processes NMEA GSV sentences (satellites in view)
 248  * \param[in] Buffer for parsed nmea GSV sentence
 249  */
 250 void NMEAParser::nmeaProcessGPGSV(char *packet)
 251 {
 252     // start parsing just after "GPGSV,"
 253     // attempt to reject empty packets right away
 254     if (packet[6] == ',' && packet[7] == ',') {
 255         return;
 256     }
 257
 258     if (!nmeaChecksum(packet)) {
 259         // checksum not valid
 260         return;
 261     }
 262     nmeaTerminateAtChecksum(packet);
 263
 264     QString nmeaString(packet);
 265     QStringList tokenslist = nmeaString.split(",");
 266
 267
 268     // Officially there should be a max of three sentences (12 sats), some gps receivers do more..
 269
 270     const int sentence_total = tokenslist.at(1).toInt(); // Number of sentences for full data
 271     const int sentence_index = tokenslist.at(2).toInt(); // sentence x of y
 272
 273     int sats = (tokenslist.size() - 4) / 4;
 274     for (int sat = 0; sat < sats; sat++) {
 275         int base          = 4 + sat * 4;
 276         const int id      = tokenslist.at(base + 0).toInt(); // Satellite PRN number
 277         const int elv     = tokenslist.at(base + 1).toInt(); // Elevation, degrees
 278         const int azimuth = tokenslist.at(base + 2).toInt(); // Azimuth, degrees
 279         const int sig     = tokenslist.at(base + 3).toInt(); // SNR - higher is better
 280         const int index   = (sentence_index - 1) * 4 + sat;
 281         emit satellite(index, id, elv, azimuth, sig);
 282     }
 283
 284     if (sentence_index == sentence_total) {
 285         // Last sentence
 286         int total_sats = (sentence_index - 1) * 4 + sats;
 287         for (int emptySatIndex = total_sats; emptySatIndex < 16; emptySatIndex++) {
 288             // Wipe the rest.
 289             emit satellite(emptySatIndex, 0, 0, 0, 0);
 290         }
 291     }
 292 }
 293
 294 /**
 295  * Prosesses NMEA GPGGA sentences
 296  * \param[in] Buffer for parsed nmea GPGGA sentence
 297  */
 298 void NMEAParser::nmeaProcessGPGGA(char *packet)
 299 {
 300     // start parsing just after "GPGGA,"
 301     // attempt to reject empty packets right away
 302     if (packet[6] == ',' && packet[7] == ',') {
 303         return;
 304     }
 305
 306     if (!nmeaChecksum(packet)) {
 307         // checksum not valid
 308         return;
 309     }
 310     nmeaTerminateAtChecksum(packet);
 311
 312     QString nmeaString(packet);
 313     QStringList tokenslist = nmeaString.split(",");
 314     GpsData.GPStime  = tokenslist.at(1).toDouble();
 315     GpsData.Latitude = tokenslist.at(2).toDouble();
 316     int deg    = (int)GpsData.Latitude / 100;
 317     double min = ((GpsData.Latitude) - (deg * 100)) / 60.0;
 318     GpsData.Latitude = deg + min;
 319     // next field: N/S indicator
 320     // correct latitute for N/S
 321     if (tokenslist.at(3).contains("S")) {
 322         GpsData.Latitude = -GpsData.Latitude;
 323     }
 324
 325     GpsData.Longitude = tokenslist.at(4).toDouble();
 326     deg = (int)GpsData.Longitude / 100;
 327     min = ((GpsData.Longitude) - (deg * 100)) / 60.0;
 328     GpsData.Longitude = deg + min;
 329     // next field: E/W indicator
 330     // correct latitute for E/W
 331     if (tokenslist.at(5).contains("W")) {
 332         GpsData.Longitude = -GpsData.Longitude;
 333     }
 334
 335     GpsData.SV = tokenslist.at(7).toInt();
 336
 337     GpsData.Altitude = tokenslist.at(9).toDouble();
 338     GpsData.GeoidSeparation = tokenslist.at(11).toDouble();
 339     emit position(GpsData.Latitude, GpsData.Longitude, GpsData.Altitude);
 340     emit sv(GpsData.SV);
 341     emit datetime(GpsData.GPSdate, GpsData.GPStime);
 342 }
 343
 344 /**
 345  * Prosesses NMEA GPRMC sentences
 346  * \param[in] Buffer for parsed nmea GPRMC sentence
 347  */
 348 void NMEAParser::nmeaProcessGPRMC(char *packet)
 349 {
 350     // start parsing just after "GPRMC,"
 351     // attempt to reject empty packets right away
 352     if (packet[6] == ',' && packet[7] == ',') {
 353         return;
 354     }
 355
 356     if (!nmeaChecksum(packet)) {
 357         // checksum not valid
 358         return;
 359     }
 360     nmeaTerminateAtChecksum(packet);
 361
 362     QString nmeaString(packet);
 363     QStringList tokenslist = nmeaString.split(",");
 364     GpsData.GPStime     = tokenslist.at(1).toDouble();
 365     GpsData.Groundspeed = tokenslist.at(7).toDouble();
 366     GpsData.Groundspeed = GpsData.Groundspeed * 0.51444;
 367     GpsData.Heading     = tokenslist.at(8).toDouble();
 368     GpsData.GPSdate     = tokenslist.at(9).toDouble();
 369     emit datetime(GpsData.GPSdate, GpsData.GPStime);
 370     emit speedheading(GpsData.Groundspeed, GpsData.Heading);
 371 }
 372
 373
 374 /**
 375  * Prosesses NMEA GPVTG sentences
 376  * \param[in] Buffer for parsed nmea GPVTG sentence
 377  */
 378 void NMEAParser::nmeaProcessGPVTG(char *packet)
 379 {
 380     // start parsing just after "GPVTG,"
 381     // attempt to reject empty packets right away
 382     if (packet[6] == ',' && packet[7] == ',') {
 383         return;
 384     }
 385
 386     if (!nmeaChecksum(packet)) {
 387         // checksum not valid
 388         return;
 389     }
 390     nmeaTerminateAtChecksum(packet);
 391
 392     QString nmeaString(packet);
 393     QStringList tokenslist = nmeaString.split(",");
 394
 395     GpsData.Heading     = tokenslist.at(1).toDouble();
 396     GpsData.Groundspeed = tokenslist.at(7).toDouble();
 397     GpsData.Groundspeed = GpsData.Groundspeed / 3.6;
 398     emit speedheading(GpsData.Groundspeed, GpsData.Heading);
 399 }
 400
 401 /**
 402  * Prosesses NMEA GPGSA sentences
 403  * \param[in] Buffer for parsed nmea GPGSA sentence
 404  */
 405 void NMEAParser::nmeaProcessGPGSA(char *packet)
 406 {
 407     // start parsing just after "GPGSA,"
 408     // attempt to reject empty packets right away
 409     if (packet[6] == ',' && packet[7] == ',') {
 410         return;
 411     }
 412
 413     if (!nmeaChecksum(packet)) {
 414         // checksum not valid
 415         return;
 416     }
 417     nmeaTerminateAtChecksum(packet);
 418
 419     QString nmeaString(packet);
 420     QStringList tokenslist = nmeaString.split(",");
 421
 422     // M=Manual, forced to operate in 2D or 3D
 423     // A=Automatic, 3D/2D
 424     QString fixmodeValue = tokenslist.at(1);
 425     if (fixmodeValue == "A") {
 426         emit fixmode(QString("Auto"));
 427     } else if (fixmodeValue == "B") {
 428         emit fixmode(QString("Manual"));
 429     }
 430
 431     // Mode: 1=Fix not available, 2=2D, 3=3D
 432     int fixtypeValue = tokenslist.at(2).toInt();
 433     if (fixtypeValue == 1) {
 434         emit fixtype(QString("NoFix"));
 435     } else if (fixtypeValue == 2) {
 436         emit fixtype(QString("Fix2D"));
 437     } else if (fixtypeValue == 3) {
 438         emit fixtype(QString("Fix3D"));
 439     }
 440
 441     // 3-14 = IDs of SVs used in position fix (null for unused fields)
 442     QList<int> svList;
 443     for (int pos = 0; pos < 12; pos++) {
 444         QString sv = tokenslist.at(3 + pos);
 445         if (!sv.isEmpty()) {
 446             svList.append(sv.toInt());
 447         }
 448     }
 449     emit fixSVs(svList);
 450
 451     // 15   = PDOP
 452     // 16   = HDOP
 453     // 17   = VDOP
 454     GpsData.PDOP = tokenslist.at(15).toDouble();
 455     GpsData.HDOP = tokenslist.at(16).toDouble();
 456     GpsData.VDOP = tokenslist.at(17).toDouble();
 457     emit dop(GpsData.HDOP, GpsData.VDOP, GpsData.PDOP);
 458 }
 459
 460 /**
 461  * Prosesses NMEA GPZDA sentences
 462  * \param[in] Buffer for parsed nmea GPZDA sentence
 463  */
 464 void NMEAParser::nmeaProcessGPZDA(char *packet)
 465 {
 466     // start parsing just after "GPZDA,"
 467     // attempt to reject empty packets right away
 468     if (packet[6] == ',' && packet[7] == ',') {
 469         return;
 470     }
 471
 472     if (!nmeaChecksum(packet)) {
 473         // checksum not valid
 474         return;
 475     }
 476     nmeaTerminateAtChecksum(packet);
 477
 478     QString nmeaString(packet);
 479     QStringList tokenslist = nmeaString.split(",");
 480
 481     GpsData.GPStime = tokenslist.at(1).toDouble();
 482     int day   = tokenslist.at(2).toInt();
 483     int month = tokenslist.at(3).toInt();
 484     int year  = tokenslist.at(4).toInt();
 485     GpsData.GPSdate = day * 10000 + month * 100 + (year - 2000);
 486     emit datetime(GpsData.GPSdate, GpsData.GPStime);
 487 }