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 // Becarefull when adding debug here
00026 // it take a lot of time to print on console
00027 // so if you do debuging please ensure your
00028 // RX buffers don't gonna ovf.
00029 #define __DEBUG__ 0
00030 #ifndef __MODULE__
00031 #define __MODULE__ "TinyGPS.cpp"
00032 #endif
00033 #include "MyDebug.h"
00034 
00035 #define _GPRMC_TERM   "GPRMC"
00036 #define _GPGGA_TERM   "GPGGA"
00037 #define _GPGSV_TERM   "GPGSV"
00038 
00039 TinyGPS::TinyGPS()
00040 :  _time(GPS_INVALID_TIME)
00041 ,  _date(GPS_INVALID_DATE)
00042 ,  _latitude(GPS_INVALID_ANGLE)
00043 ,  _longitude(GPS_INVALID_ANGLE)
00044 ,  _altitude(GPS_INVALID_ALTITUDE)
00045 ,  _speed(GPS_INVALID_SPEED)
00046 ,  _course(GPS_INVALID_ANGLE)
00047 ,  _hdop(0)
00048 ,  _sat_count(0)
00049 ,  _last_time_fix(GPS_INVALID_FIX_TIME)
00050 ,  _last_position_fix(GPS_INVALID_FIX_TIME)
00051 ,  _parity(0)
00052 ,  _is_checksum_term(false)
00053 ,  _sentence_type(_GPS_SENTENCE_OTHER)
00054 ,  _term_number(0)
00055 ,  _term_offset(0)
00056 ,  _gps_data_good(false)
00057 ,  _rmc_ready(false)
00058 ,  _gga_ready(false)
00059 ,  _gsv_ready(false)
00060 #ifndef _GPS_NO_STATS
00061 ,  _encoded_characters(0)
00062 ,  _good_sentences(0)
00063 ,  _failed_checksum(0)
00064 #endif
00065 {
00066   _term[0] = '\0';
00067 }
00068 
00069 //
00070 // public methods
00071 //
00072 
00073 bool TinyGPS::encode(char c)
00074 {
00075   bool valid_sentence = false;
00076 
00077   DBG("INSERTING %c",c);
00078 
00079   ++_encoded_characters;
00080   switch(c)
00081   {
00082   case ',': // term terminators
00083     _parity ^= c;
00084   case '\r':
00085   case '\n':
00086   case '*':
00087     DBG("*END");
00088     if (_term_offset < sizeof(_term))
00089     {
00090       _term[_term_offset] = 0;
00091       valid_sentence = term_complete();
00092     }
00093     ++_term_number;
00094     _term_offset = 0;
00095     _is_checksum_term = c == '*';
00096     return valid_sentence;
00097 
00098   case '$': // sentence begin
00099     DBG("$START");
00100     _term_number = _term_offset = 0;
00101     _parity = 0;
00102     _sentence_type = _GPS_SENTENCE_OTHER;
00103     _is_checksum_term = false;
00104     _gps_data_good = false;
00105     return valid_sentence;
00106   }
00107 
00108   // ordinary characters
00109   if (_term_offset < sizeof(_term) - 1)
00110     _term[_term_offset++] = c;
00111   if (!_is_checksum_term)
00112     _parity ^= c;
00113 
00114   return valid_sentence;
00115 }
00116 
00117 #ifndef _GPS_NO_STATS
00118 void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
00119 {
00120   if (chars) *chars = _encoded_characters;
00121   if (sentences) *sentences = _good_sentences;
00122   if (failed_cs) *failed_cs = _failed_checksum;
00123 }
00124 #endif
00125 
00126 //
00127 // internal utilities
00128 //
00129 int TinyGPS::from_hex(char a) 
00130 {
00131   if (a >= 'A' && a <= 'F')
00132     return a - 'A' + 10;
00133   else if (a >= 'a' && a <= 'f')
00134     return a - 'a' + 10;
00135   else
00136     return a - '0';
00137 }
00138 
00139 unsigned long TinyGPS::parse_decimal()
00140 {
00141   char *p = _term;
00142   bool isneg = *p == '-';
00143   if (isneg) ++p;
00144   unsigned long ret = 100UL * gpsatol(p);
00145   while (gpsisdigit(*p)) ++p;
00146   if (*p == '.')
00147   {
00148     if (gpsisdigit(p[1]))
00149     {
00150       ret += 10 * (p[1] - '0');
00151       if (gpsisdigit(p[2]))
00152         ret += p[2] - '0';
00153     }
00154   }
00155   return isneg ? -ret : ret;
00156 }
00157 
00158 unsigned long TinyGPS::parse_degrees()
00159 {
00160   char *p;
00161   unsigned long left = gpsatol(_term);
00162   unsigned long tenk_minutes = (left % 100UL) * 10000UL;
00163   for (p=_term; gpsisdigit(*p); ++p);
00164   if (*p == '.')
00165   {
00166     unsigned long mult = 1000;
00167     while (gpsisdigit(*++p))
00168     {
00169       tenk_minutes += mult * (*p - '0');
00170       mult /= 10;
00171     }
00172   }
00173   return (left / 100) * 100000 + tenk_minutes / 6;
00174 }
00175 
00176 // Processes a just-completed term
00177 // Returns true if new sentence has just passed checksum test and is validated
00178 bool TinyGPS::term_complete()
00179 {
00180   if (_is_checksum_term)
00181   {
00182     byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
00183     if (checksum == _parity)
00184     {
00185       if (_gps_data_good)
00186       {
00187 #ifndef _GPS_NO_STATS
00188         ++_good_sentences;
00189 #endif
00190         _last_time_fix = _new_time_fix;
00191         _last_position_fix = _new_position_fix;
00192 
00193         switch(_sentence_type)
00194         {
00195         case _GPS_SENTENCE_GPRMC:
00196           _time      = _new_time;
00197           _date      = _new_date;
00198           _latitude  = _new_latitude;
00199           _longitude = _new_longitude;
00200           _speed     = _new_speed;
00201           _course    = _new_course;
00202           _rmc_ready = true;
00203           break;
00204         case _GPS_SENTENCE_GPGGA:
00205           _altitude  = _new_altitude;
00206           _time      = _new_time;
00207           _latitude  = _new_latitude;
00208           _longitude = _new_longitude;
00209           _gga_ready = true;
00210           _hdop      = _new_hdop;
00211           _sat_count = _new_sat_count;
00212           DBG("YEAH GPGGA");
00213         case _GPS_SENTENCE_GPGSV:
00214           _gsv_ready = true;
00215           break;
00216         }
00217 
00218         return true;
00219       }
00220     }
00221 
00222 #ifndef _GPS_NO_STATS
00223     else
00224       ++_failed_checksum;
00225 #endif
00226     return false;
00227   }
00228 
00229   // the first term determines the sentence type
00230   if (_term_number == 0)
00231   {
00232     if (!gpsstrcmp(_term, _GPRMC_TERM))
00233       _sentence_type = _GPS_SENTENCE_GPRMC;
00234     else if (!gpsstrcmp(_term, _GPGGA_TERM))
00235       _sentence_type = _GPS_SENTENCE_GPGGA;
00236     else if (!gpsstrcmp(_term, _GPGSV_TERM))
00237       _sentence_type = _GPS_SENTENCE_GPGSV;
00238     else
00239       _sentence_type = _GPS_SENTENCE_OTHER;
00240     return false;
00241   }
00242 
00243   if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
00244   switch((_sentence_type == _GPS_SENTENCE_GPGGA ? 200 : 100) + _term_number)
00245   {
00246     case 101: // Time in both sentences
00247     case 201:
00248       _new_time = parse_decimal();
00249       _new_time_fix = millis();
00250       break;
00251     case 102: // GPRMC validity
00252       _gps_data_good = _term[0] == 'A';
00253       break;
00254     case 103: // Latitude
00255     case 202:
00256       DBG("GOT LAT");
00257       _new_latitude = parse_degrees();
00258       _new_position_fix = millis();
00259       break;
00260     case 104: // N/S
00261     case 203:
00262       if (_term[0] == 'S')
00263         _new_latitude = -_new_latitude;
00264       break;
00265     case 105: // Longitude
00266     case 204:
00267       _new_longitude = parse_degrees();
00268       break;
00269     case 106: // E/W
00270     case 205:
00271       if (_term[0] == 'W')
00272         _new_longitude = -_new_longitude;
00273       break;
00274     case 107: // Speed (GPRMC)
00275       _new_speed = parse_decimal();
00276       break;
00277     case 108: // Course (GPRMC)
00278       _new_course = parse_decimal();
00279       break;
00280     case 109: // Date (GPRMC)
00281       _new_date = gpsatol(_term);
00282       break;
00283     case 206: // Fix data (GPGGA)
00284       _gps_data_good = _term[0] > '0';
00285       break;
00286     case 207: // Number of satelites tracked (GPGGA)
00287       _new_sat_count = parse_decimal();
00288       break;
00289     case 208: // Horizontal Dilution of Position (GPGGA)
00290       _new_hdop = parse_decimal();
00291       break;
00292     case 209: // Altitude (GPGGA)
00293       _new_altitude = parse_decimal();
00294       break;
00295   } /* switch */
00296 
00297   return false;
00298 }
00299 
00300 long TinyGPS::gpsatol(const char *str)
00301 {
00302   long ret = 0;
00303   while (gpsisdigit(*str))
00304     ret = 10 * ret + *str++ - '0';
00305   return ret;
00306 }
00307 
00308 int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
00309 {
00310   while (*str1 && *str1 == *str2)
00311     ++str1, ++str2;
00312   return *str1;
00313 }