Diff: TinyGPS.cpp

Revision:

0:8a347288f82c

Child:

1:f522b8bdf987

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TinyGPS.cpp	Wed Apr 27 19:21:28 2011 +0000
@@ -0,0 +1,297 @@
+/*
+  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
+  
+  Ported to mbed by Michael Shimniok http://www.bot-thoughts.com/
+*/
+
+#include "TinyGPS.h"
+
+#define _GPRMC_TERM   "GPRMC"
+#define _GPGGA_TERM   "GPGGA"
+#define _GPGSV_TERM   "GPGSV"
+
+TinyGPS::TinyGPS()
+:  _time(GPS_INVALID_TIME)
+,  _date(GPS_INVALID_DATE)
+,  _latitude(GPS_INVALID_ANGLE)
+,  _longitude(GPS_INVALID_ANGLE)
+,  _altitude(GPS_INVALID_ALTITUDE)
+,  _speed(GPS_INVALID_SPEED)
+,  _course(GPS_INVALID_ANGLE)
+,  _hdop(0)
+,  _sat_count(0)
+,  _last_time_fix(GPS_INVALID_FIX_TIME)
+,  _last_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)
+,  _gsv_ready(false)
+#ifndef _GPS_NO_STATS
+,  _encoded_characters(0)
+,  _good_sentences(0)
+,  _failed_checksum(0)
+#endif
+{
+  _term[0] = '\0';
+}
+
+//
+// public methods
+//
+
+bool TinyGPS::encode(char c)
+{
+  bool valid_sentence = false;
+
+  ++_encoded_characters;
+  switch(c)
+  {
+  case ',': // term terminators
+    _parity ^= c;
+  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 == '*';
+    return valid_sentence;
+
+  case '$': // sentence begin
+    _term_number = _term_offset = 0;
+    _parity = 0;
+    _sentence_type = _GPS_SENTENCE_OTHER;
+    _is_checksum_term = false;
+    _gps_data_good = false;
+    return valid_sentence;
+  }
+
+  // ordinary characters
+  if (_term_offset < sizeof(_term) - 1)
+    _term[_term_offset++] = c;
+  if (!_is_checksum_term)
+    _parity ^= c;
+
+  return valid_sentence;
+}
+
+#ifndef _GPS_NO_STATS
+void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
+{
+  if (chars) *chars = _encoded_characters;
+  if (sentences) *sentences = _good_sentences;
+  if (failed_cs) *failed_cs = _failed_checksum;
+}
+#endif
+
+//
+// internal utilities
+//
+int TinyGPS::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';
+}
+
+unsigned long TinyGPS::parse_decimal()
+{
+  char *p = _term;
+  bool isneg = *p == '-';
+  if (isneg) ++p;
+  unsigned long ret = 100UL * gpsatol(p);
+  while (gpsisdigit(*p)) ++p;
+  if (*p == '.')
+  {
+    if (gpsisdigit(p[1]))
+    {
+      ret += 10 * (p[1] - '0');
+      if (gpsisdigit(p[2]))
+        ret += p[2] - '0';
+    }
+  }
+  return isneg ? -ret : ret;
+}
+
+unsigned long TinyGPS::parse_degrees()
+{
+  char *p;
+  unsigned long left = gpsatol(_term);
+  unsigned long tenk_minutes = (left % 100UL) * 10000UL;
+  for (p=_term; gpsisdigit(*p); ++p);
+  if (*p == '.')
+  {
+    unsigned long mult = 1000;
+    while (gpsisdigit(*++p))
+    {
+      tenk_minutes += mult * (*p - '0');
+      mult /= 10;
+    }
+  }
+  return (left / 100) * 100000 + tenk_minutes / 6;
+}
+
+// Processes a just-completed term
+// Returns true if new sentence has just passed checksum test and is validated
+bool TinyGPS::term_complete()
+{
+  if (_is_checksum_term)
+  {
+    byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
+    if (checksum == _parity)
+    {
+      if (_gps_data_good || 1) // I want to use the thing in demo mode
+      {
+#ifndef _GPS_NO_STATS
+        ++_good_sentences;
+#endif
+        _last_time_fix = _new_time_fix;
+        _last_position_fix = _new_position_fix;
+
+        switch(_sentence_type)
+        {
+        case _GPS_SENTENCE_GPRMC:
+          _time      = _new_time;
+          _date      = _new_date;
+          _latitude  = _new_latitude;
+          _longitude = _new_longitude;
+          _speed     = _new_speed;
+          _course    = _new_course;
+          _rmc_ready = true;
+          break;
+        case _GPS_SENTENCE_GPGGA:
+          _altitude  = _new_altitude;
+          _time      = _new_time;
+          _latitude  = _new_latitude;
+          _longitude = _new_longitude;
+          _gga_ready = true;
+          _hdop      = _new_hdop;
+          _sat_count = _new_sat_count;
+        case _GPS_SENTENCE_GPGSV:
+          _gsv_ready = true;
+          break;
+        }
+
+        return true;
+      }
+    }
+
+#ifndef _GPS_NO_STATS
+    else
+      ++_failed_checksum;
+#endif
+    return false;
+  }
+
+  // the first term determines the sentence type
+  if (_term_number == 0)
+  {
+    if (!gpsstrcmp(_term, _GPRMC_TERM))
+      _sentence_type = _GPS_SENTENCE_GPRMC;
+    else if (!gpsstrcmp(_term, _GPGGA_TERM))
+      _sentence_type = _GPS_SENTENCE_GPGGA;
+    else if (!gpsstrcmp(_term, _GPGSV_TERM))
+      _sentence_type = _GPS_SENTENCE_GPGSV;
+    else
+      _sentence_type = _GPS_SENTENCE_OTHER;
+    return false;
+  }
+
+  if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
+  switch((_sentence_type == _GPS_SENTENCE_GPGGA ? 200 : 100) + _term_number)
+  {
+    case 101: // Time in both sentences
+    case 201:
+      _new_time = parse_decimal();
+      _new_time_fix = millis();
+      break;
+    case 102: // GPRMC validity
+      _gps_data_good = _term[0] == 'A';
+      break;
+    case 103: // Latitude
+    case 202:
+      _new_latitude = parse_degrees();
+      _new_position_fix = millis();
+      break;
+    case 104: // N/S
+    case 203:
+      if (_term[0] == 'S')
+        _new_latitude = -_new_latitude;
+      break;
+    case 105: // Longitude
+    case 204:
+      _new_longitude = parse_degrees();
+      break;
+    case 106: // E/W
+    case 205:
+      if (_term[0] == 'W')
+        _new_longitude = -_new_longitude;
+      break;
+    case 107: // Speed (GPRMC)
+      _new_speed = parse_decimal();
+      break;
+    case 108: // Course (GPRMC)
+      _new_course = parse_decimal();
+      break;
+    case 109: // Date (GPRMC)
+      _new_date = gpsatol(_term);
+      break;
+    case 206: // Fix data (GPGGA)
+      _gps_data_good = _term[0] > '0';
+      break;
+    case 207: // Number of satelites tracked (GPGGA)
+      _new_sat_count = parse_decimal();
+      break;
+    case 208: // Horizontal Dilution of Position (GPGGA)
+      _new_hdop = parse_decimal();
+      break;
+    case 209: // Altitude (GPGGA)
+      _new_altitude = parse_decimal();
+      break;
+  } /* switch */
+
+  return false;
+}
+
+long TinyGPS::gpsatol(const char *str)
+{
+  long ret = 0;
+  while (gpsisdigit(*str))
+    ret = 10 * ret + *str++ - '0';
+  return ret;
+}
+
+int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
+{
+  while (*str1 && *str1 == *str2)
+    ++str1, ++str2;
+  return *str1;
+}