TinyGPS.cpp

00001 /*
00002   TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
00003   Copyright (C) 2008-9 Mikal Hart
00004   All rights reserved.
00005 
00006   This library is free software; you can redistribute it and/or
00007   modify it under the terms of the GNU Lesser General Public
00008   License as published by the Free Software Foundation; either
00009   version 2.1 of the License, or (at your option) any later version.
00010 
00011   This library is distributed in the hope that it will be useful,
00012   but WITHOUT ANY WARRANTY; without even the implied warranty of
00013   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014   Lesser General Public License for more details.
00015 
00016   You should have received a copy of the GNU Lesser General Public
00017   License along with this library; if not, write to the Free Software
00018   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00019   
00020   Ported to mbed by Michael Shimniok http://www.bot-thoughts.com/
00021 */
00022 
00023 #include "TinyGPS.h"
00024 
00025 #define _GPRMC_TERM   "GPRMC"
00026 #define _GPGGA_TERM   "GPGGA"
00027 #define _GPGSV_TERM   "GPGSV"
00028 
00029 TinyGPS::TinyGPS()
00030 :  _time(GPS_INVALID_TIME)
00031 ,  _date(GPS_INVALID_DATE)
00032 ,  _latitude(GPS_INVALID_ANGLE)
00033 ,  _longitude(GPS_INVALID_ANGLE)
00034 ,  _altitude(GPS_INVALID_ALTITUDE)
00035 ,  _speed(GPS_INVALID_SPEED)
00036 ,  _course(GPS_INVALID_ANGLE)
00037 ,  _hdop(0)
00038 ,  _sat_count(0)
00039 ,  _last_time_fix(GPS_INVALID_FIX_TIME)
00040 ,  _last_position_fix(GPS_INVALID_FIX_TIME)
00041 ,  _parity(0)
00042 ,  _is_checksum_term(false)
00043 ,  _sentence_type(_GPS_SENTENCE_OTHER)
00044 ,  _term_number(0)
00045 ,  _term_offset(0)
00046 ,  _gps_data_good(false)
00047 ,  _rmc_ready(false)
00048 ,  _gga_ready(false)
00049 ,  _gsv_ready(false)
00050 #ifndef _GPS_NO_STATS
00051 ,  _encoded_characters(0)
00052 ,  _good_sentences(0)
00053 ,  _failed_checksum(0)
00054 #endif
00055 {
00056   _term[0] = '\0';
00057 }
00058 
00059 //
00060 // public methods
00061 //
00062 
00063 bool TinyGPS::encode(char c)
00064 {
00065   bool valid_sentence = false;
00066 
00067   ++_encoded_characters;
00068   switch(c)
00069   {
00070   case ',': // term terminators
00071     _parity ^= c;
00072   case '\r':
00073   case '\n':
00074   case '*':
00075     if (_term_offset < sizeof(_term))
00076     {
00077       _term[_term_offset] = 0;
00078       valid_sentence = term_complete();
00079     }
00080     ++_term_number;
00081     _term_offset = 0;
00082     _is_checksum_term = c == '*';
00083     return valid_sentence;
00084 
00085   case '$': // sentence begin
00086     _term_number = _term_offset = 0;
00087     _parity = 0;
00088     _sentence_type = _GPS_SENTENCE_OTHER;
00089     _is_checksum_term = false;
00090     _gps_data_good = false;
00091     return valid_sentence;
00092   }
00093 
00094   // ordinary characters
00095   if (_term_offset < sizeof(_term) - 1)
00096     _term[_term_offset++] = c;
00097   if (!_is_checksum_term)
00098     _parity ^= c;
00099 
00100   return valid_sentence;
00101 }
00102 
00103 #ifndef _GPS_NO_STATS
00104 void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
00105 {
00106   if (chars) *chars = _encoded_characters;
00107   if (sentences) *sentences = _good_sentences;
00108   if (failed_cs) *failed_cs = _failed_checksum;
00109 }
00110 #endif
00111 
00112 //
00113 // internal utilities
00114 //
00115 int TinyGPS::from_hex(char a) 
00116 {
00117   if (a >= 'A' && a <= 'F')
00118     return a - 'A' + 10;
00119   else if (a >= 'a' && a <= 'f')
00120     return a - 'a' + 10;
00121   else
00122     return a - '0';
00123 }
00124 
00125 unsigned long TinyGPS::parse_decimal()
00126 {
00127   char *p = _term;
00128   bool isneg = *p == '-';
00129   if (isneg) ++p;
00130   unsigned long ret = 100UL * gpsatol(p);
00131   while (gpsisdigit(*p)) ++p;
00132   if (*p == '.')
00133   {
00134     if (gpsisdigit(p[1]))
00135     {
00136       ret += 10 * (p[1] - '0');
00137       if (gpsisdigit(p[2]))
00138         ret += p[2] - '0';
00139     }
00140   }
00141   return isneg ? -ret : ret;
00142 }
00143 
00144 unsigned long TinyGPS::parse_degrees()
00145 {
00146   char *p;
00147   unsigned long left = gpsatol(_term);
00148   unsigned long tenk_minutes = (left % 100UL) * 10000UL;
00149   for (p=_term; gpsisdigit(*p); ++p);
00150   if (*p == '.')
00151   {
00152     unsigned long mult = 1000;
00153     while (gpsisdigit(*++p))
00154     {
00155       tenk_minutes += mult * (*p - '0');
00156       mult /= 10;
00157     }
00158   }
00159   return (left / 100) * 100000 + tenk_minutes / 6;
00160 }
00161 
00162 // Processes a just-completed term
00163 // Returns true if new sentence has just passed checksum test and is validated
00164 bool TinyGPS::term_complete()
00165 {
00166   if (_is_checksum_term)
00167   {
00168     byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
00169     if (checksum == _parity)
00170     {
00171       if (_gps_data_good)
00172       {
00173 #ifndef _GPS_NO_STATS
00174         ++_good_sentences;
00175 #endif
00176         _last_time_fix = _new_time_fix;
00177         _last_position_fix = _new_position_fix;
00178 
00179         switch(_sentence_type)
00180         {
00181         case _GPS_SENTENCE_GPRMC:
00182           _time      = _new_time;
00183           _date      = _new_date;
00184           _latitude  = _new_latitude;
00185           _longitude = _new_longitude;
00186           _speed     = _new_speed;
00187           _course    = _new_course;
00188           _rmc_ready = true;
00189           break;
00190         case _GPS_SENTENCE_GPGGA:
00191           _altitude  = _new_altitude;
00192           _time      = _new_time;
00193           _latitude  = _new_latitude;
00194           _longitude = _new_longitude;
00195           _gga_ready = true;
00196           _hdop      = _new_hdop;
00197           _sat_count = _new_sat_count;
00198         case _GPS_SENTENCE_GPGSV:
00199           _gsv_ready = true;
00200           break;
00201         }
00202 
00203         return true;
00204       }
00205     }
00206 
00207 #ifndef _GPS_NO_STATS
00208     else
00209       ++_failed_checksum;
00210 #endif
00211     return false;
00212   }
00213 
00214   // the first term determines the sentence type
00215   if (_term_number == 0)
00216   {
00217     if (!gpsstrcmp(_term, _GPRMC_TERM))
00218       _sentence_type = _GPS_SENTENCE_GPRMC;
00219     else if (!gpsstrcmp(_term, _GPGGA_TERM))
00220       _sentence_type = _GPS_SENTENCE_GPGGA;
00221     else if (!gpsstrcmp(_term, _GPGSV_TERM))
00222       _sentence_type = _GPS_SENTENCE_GPGSV;
00223     else
00224       _sentence_type = _GPS_SENTENCE_OTHER;
00225     return false;
00226   }
00227 
00228   if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
00229   switch((_sentence_type == _GPS_SENTENCE_GPGGA ? 200 : 100) + _term_number)
00230   {
00231     case 101: // Time in both sentences
00232     case 201:
00233       _new_time = parse_decimal();
00234       _new_time_fix = millis();
00235       break;
00236     case 102: // GPRMC validity
00237       _gps_data_good = _term[0] == 'A';
00238       break;
00239     case 103: // Latitude
00240     case 202:
00241       _new_latitude = parse_degrees();
00242       _new_position_fix = millis();
00243       break;
00244     case 104: // N/S
00245     case 203:
00246       if (_term[0] == 'S')
00247         _new_latitude = -_new_latitude;
00248       break;
00249     case 105: // Longitude
00250     case 204:
00251       _new_longitude = parse_degrees();
00252       break;
00253     case 106: // E/W
00254     case 205:
00255       if (_term[0] == 'W')
00256         _new_longitude = -_new_longitude;
00257       break;
00258     case 107: // Speed (GPRMC)
00259       _new_speed = parse_decimal();
00260       break;
00261     case 108: // Course (GPRMC)
00262       _new_course = parse_decimal();
00263       break;
00264     case 109: // Date (GPRMC)
00265       _new_date = gpsatol(_term);
00266       break;
00267     case 206: // Fix data (GPGGA)
00268       _gps_data_good = _term[0] > '0';
00269       break;
00270     case 207: // Number of satelites tracked (GPGGA)
00271       _new_sat_count = parse_decimal();
00272       break;
00273     case 208: // Horizontal Dilution of Position (GPGGA)
00274       _new_hdop = parse_decimal();
00275       break;
00276     case 209: // Altitude (GPGGA)
00277       _new_altitude = parse_decimal();
00278       break;
00279   } /* switch */
00280 
00281   return false;
00282 }
00283 
00284 long TinyGPS::gpsatol(const char *str)
00285 {
00286   long ret = 0;
00287   while (gpsisdigit(*str))
00288     ret = 10 * ret + *str++ - '0';
00289   return ret;
00290 }
00291 
00292 int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
00293 {
00294   while (*str1 && *str1 == *str2)
00295     ++str1, ++str2;
00296   return *str1;
00297 }