Diff: GPS/NMEA.cpp

Revision:

44:0d72a8a1288a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS/NMEA.cpp	Mon Jan 07 16:47:33 2019 +0000
@@ -0,0 +1,285 @@
+/** NMEA - a small NMEA-parsing library based on TinyGPS
+ * @Author Michael Shimniok
+ * www.bot-thoughts.com
+
+ * TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
+ * Copyright (C) 2008-9 Mikal Hart
+ * All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library 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
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include "NMEA.h"
+#include <cstdlib>
+#include <stdio.h>
+#include <string.h>
+
+#define _GPRMC_TERM   "GPRMC"
+#define _GPGGA_TERM   "GPGGA"
+
+NMEA::NMEA()
+:  _new_time(GPS_INVALID_TIME)
+,  _new_date(GPS_INVALID_DATE)
+,  _new_latitude(GPS_INVALID_ANGLE)
+,  _new_longitude(GPS_INVALID_ANGLE)
+,  _new_altitude(GPS_INVALID_ALTITUDE)
+,  _new_speed(GPS_INVALID_SPEED)
+,  _new_course(GPS_INVALID_ANGLE)
+,  _new_hdop(0)
+,  _new_sat_count(0)
+,  _last_time_fix(GPS_INVALID_FIX_TIME)
+,  _new_time_fix(GPS_INVALID_FIX_TIME)
+,  _last_position_fix(GPS_INVALID_FIX_TIME)
+,  _new_position_fix(GPS_INVALID_FIX_TIME)
+,  _parity(0)
+,  _is_checksum_term(false)
+,  _sentence_type(_GPS_SENTENCE_OTHER)
+,  _term_number(0)
+,  _term_offset(0)
+,  _gps_data_good(false)
+,  _rmc_ready(false)
+,  _gga_ready(false)
+{
+  _term[0] = '\0';
+}
+
+/////////////////////////////////////////////////////////////////////////////////
+// Public member functions
+//
+
+// Parse NMEA once character at a time
+// @return 1 if GGA and RMC sentences are valid and ready
+int NMEA::parse(char c)
+{
+    int valid_sentence = 0;
+
+    switch(c) {
+    case ',': // term terminators
+        _parity ^= c;
+        // no break
+    case '\r':
+    case '\n':
+    case '*':
+        if (_term_offset < sizeof(_term)) {
+            _term[_term_offset] = 0;
+            valid_sentence = term_complete();
+        }
+        ++_term_number;
+        _term_offset = 0;
+        _is_checksum_term = c == '*';
+#ifdef __MBED__
+        if (_callback) _callback();
+#endif
+        break;
+
+    case '$': // sentence begin
+        _term_number = _term_offset = 0;
+        _parity = 0;
+        _sentence_type = _GPS_SENTENCE_OTHER;
+        _is_checksum_term = false;
+        _gps_data_good = false;
+        break;
+
+    default:
+        // ordinary characters
+        if (_term_offset < sizeof(_term) - 1)
+            _term[_term_offset++] = c;
+        if (!_is_checksum_term)
+            _parity ^= c;
+        }
+
+    return valid_sentence & ready();
+}
+
+
+/////////////////////////////////////////////////////////////////////////////////
+// Private member functions
+
+// Convert hex char representation to int
+int NMEA::from_hex(char a)
+{
+    if (a >= 'A' && a <= 'F')
+        return a - 'A' + 10;
+    else if (a >= 'a' && a <= 'f')
+        return a - 'a' + 10;
+    else
+        return a - '0';
+}
+
+
+// Convert string to integer
+int NMEA::parse_int()
+{
+    char *p = _term;
+    bool isneg = *p == '-';
+    if (isneg) ++p;
+    int ret = atoi(p);
+    return isneg ? -ret : ret;
+}
+
+
+// Convert string to double
+double NMEA::parse_decimal()
+{
+  char *p = _term;
+  double ret = atof(p);
+  return ret;
+}
+
+
+// Convert NMEA lat/lon degrees/minutes to double degrees
+double NMEA::parse_degrees()
+{
+  double result;
+  int16_t degrees = atoi(_term) / 100;
+  char *minutes = strchr(_term, '.') - 2;
+  //printf("term=<%s> %d %f\n", _term, degrees, atof(minutes));
+  result = degrees + atof(minutes) / 60.0;
+  return result;
+}
+
+
+// Processes a just-completed term
+// @return true if new sentence has just passed checksum test and is validated
+bool NMEA::term_complete()
+{
+    if (_is_checksum_term) {
+        byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
+        if (checksum == _parity) {
+            if (_gps_data_good) {
+                _last_time_fix = _new_time_fix;
+                _last_position_fix = _new_position_fix;
+                switch(_sentence_type) {
+                    case _GPS_SENTENCE_GPRMC:
+                        latest.date   = _new_date;
+                        latest.month  = _new_month;
+                        latest.year   = _new_year;
+                        latest.hour   = _new_hour;
+                        latest.minute = _new_minute;
+                        latest.second = _new_second;
+                        latest.lat    = _new_latitude;
+                        latest.lon    = _new_longitude;
+                        latest.speed  = _new_speed;
+                        latest.course = _new_course;
+                        printf("--- RMC_READY\n");
+                        _rmc_ready = true;
+                        break;
+                    case _GPS_SENTENCE_GPGGA:
+                        _altitude      = _new_altitude;
+                        latest.lat     = _new_latitude;
+                        latest.lon     = _new_longitude;
+                        latest.hdop    = _new_hdop;
+                        latest.svcount = _new_sat_count;
+                        printf("--- GGA_READY\n");
+                        _gga_ready = true;
+                        break;
+                }//switch _sentence_type
+                return true;
+//            } else {
+//                printf("bad data\n");
+            }//if _gps_data_good
+        } else {
+            printf("bad checksum 0x%x expected 0x%x\n", checksum, _parity);
+        }//if checksum
+
+        return false;
+    }//if _is_checksum_term
+
+    // the first term determines the sentence type
+    if (_term_number == 0) {
+        if (!strcmp(_term, _GPRMC_TERM))
+            _sentence_type = _GPS_SENTENCE_GPRMC;
+        else if (!strcmp(_term, _GPGGA_TERM))
+            _sentence_type = _GPS_SENTENCE_GPGGA;
+        else
+            _sentence_type = _GPS_SENTENCE_OTHER;
+        return false;
+    }
+
+    if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
+        printf("\tterm:<%s>\n", _term);
+        switch (_sentence_type | _term_number) {
+        case _GPS_SENTENCE_GPRMC|1: // Time in both sentences
+            //case _GPS_SENTENCE_GPGGA|1:
+            {
+                char *s = _term+4;
+                _new_second = atoi(s);
+                *s = 0;
+                s -= 2;
+                _new_minute = atoi(s);
+                *s = 0;
+                s -= 2;
+                _new_hour = atoi(s);
+            }
+            break;
+        case _GPS_SENTENCE_GPRMC|2: // GPRMC validity
+          _gps_data_good = _term[0] == 'A';
+          break;
+        case _GPS_SENTENCE_GPRMC|3: // Latitude
+        case _GPS_SENTENCE_GPGGA|2:
+          _new_latitude = parse_degrees();
+          break;
+        case _GPS_SENTENCE_GPRMC|4: // N/S
+        case _GPS_SENTENCE_GPGGA|3:
+          if (_term[0] == 'S')
+            _new_latitude = -_new_latitude;
+          break;
+        case _GPS_SENTENCE_GPRMC|5: // Longitude
+        case _GPS_SENTENCE_GPGGA|4:
+          _new_longitude = parse_degrees();
+          break;
+        case _GPS_SENTENCE_GPRMC|6: // E/W
+        case _GPS_SENTENCE_GPGGA|5:
+          if (_term[0] == 'W')
+            _new_longitude = -_new_longitude;
+          break;
+        case _GPS_SENTENCE_GPRMC|7: // Speed (GPRMC)
+          _new_speed = parse_decimal();
+          break;
+        case _GPS_SENTENCE_GPRMC|8: // Course (GPRMC)
+          _new_course = parse_decimal();
+          break;
+        case _GPS_SENTENCE_GPRMC|9: // Date (GPRMC)
+            {
+                char *s = _term+4;
+                _new_year = atoi(s);
+                *s = 0;
+                s -= 2;
+                _new_month = atoi(s);
+                *s = 0;
+                s -= 2;
+                _new_date = atoi(s);
+                //printf("%02d:%02d:%02d %02d/%02d/%02d\n", _new_hour, _new_minute, _new_second, _new_month, _new_date, _new_year);
+            }
+            break;
+        case _GPS_SENTENCE_GPGGA|6: // Fix data (GPGGA)
+          _gps_data_good = _term[0] > '0';
+          break;
+        case _GPS_SENTENCE_GPGGA|7: // Number of satelites tracked (GPGGA)
+          _new_sat_count = parse_int();
+          break;
+        case _GPS_SENTENCE_GPGGA|8: // Horizontal Dilution of Position (GPGGA)
+          _new_hdop = parse_decimal();
+          break;
+        case _GPS_SENTENCE_GPGGA|9: // Altitude (GPGGA)
+          _new_altitude = parse_decimal();
+          break;
+        default :
+          break;
+        } /* switch */
+    }//if
+
+  return false;
+}