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 int TinyGPS::parse_int()
00126 {
00127   char *p = _term;
00128   bool isneg = *p == '-';
00129   if (isneg) ++p;
00130   while (*p == '0') ++p;
00131   int ret = gpsatol(p);
00132   return isneg ? -ret : ret;
00133 }
00134 
00135 unsigned long TinyGPS::parse_decimal()
00136 {
00137   char *p = _term;
00138   bool isneg = *p == '-';
00139   if (isneg) ++p;
00140   unsigned long ret = 100UL * gpsatol(p);
00141   while (gpsisdigit(*p)) ++p;
00142   if (*p == '.')
00143   {
00144     if (gpsisdigit(p[1]))
00145     {
00146       ret += 10 * (p[1] - '0');
00147       if (gpsisdigit(p[2]))
00148         ret += p[2] - '0';
00149     }
00150   }
00151   return isneg ? -ret : ret; // TODO:  can't return - when we're returning an unsigned!
00152 }
00153 
00154 // mes 04/27/12 increased fractional precision to 7 digits, was 5
00155 unsigned long TinyGPS::parse_degrees()
00156 {
00157   char *p;
00158   unsigned long left = gpsatol(_term);
00159   unsigned long tenk_minutes = (left % 100UL) * 1000000UL;
00160   for (p=_term; gpsisdigit(*p); ++p);
00161   if (*p == '.')
00162   {
00163     unsigned long mult = 100000;
00164     while (gpsisdigit(*++p))
00165     {
00166       tenk_minutes += mult * (*p - '0');
00167       mult /= 10;
00168     }
00169   }
00170   return (left / 100) * 10000000 + tenk_minutes / 6;
00171 }
00172 
00173 // Processes a just-completed term
00174 // Returns true if new sentence has just passed checksum test and is validated
00175 bool TinyGPS::term_complete()
00176 {
00177     if (_is_checksum_term) {
00178     
00179         byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
00180         if (checksum == _parity) {
00181         
00182             if (_gps_data_good) {
00183             
00184 #ifndef _GPS_NO_STATS
00185             ++_good_sentences;
00186 #endif
00187             _last_time_fix = _new_time_fix;
00188             _last_position_fix = _new_position_fix;
00189 
00190             switch(_sentence_type) {
00191             case _GPS_SENTENCE_GPRMC:
00192                 _time      = _new_time;
00193                 _date      = _new_date;
00194                 _latitude  = _new_latitude;
00195                 _longitude = _new_longitude;
00196                 _speed     = _new_speed;
00197                 _course    = _new_course;
00198                 _rmc_ready = true;
00199                 break;
00200             case _GPS_SENTENCE_GPGGA:
00201                 _altitude  = _new_altitude;
00202                 _time      = _new_time;
00203                 _latitude  = _new_latitude;
00204                 _longitude = _new_longitude;
00205                 _gga_ready = true;
00206                 _hdop      = _new_hdop;
00207                 _sat_count = _new_sat_count;
00208             case _GPS_SENTENCE_GPGSV:
00209                 _gsv_ready = true;
00210                 break;
00211             }
00212 
00213             return true;
00214         }
00215     }
00216 
00217 #ifndef _GPS_NO_STATS
00218     else
00219         ++_failed_checksum;
00220 #endif
00221     return false;
00222     }
00223 
00224     // the first term determines the sentence type
00225     if (_term_number == 0) {
00226         if (!gpsstrcmp(_term, _GPRMC_TERM))
00227             _sentence_type = _GPS_SENTENCE_GPRMC;
00228         else if (!gpsstrcmp(_term, _GPGGA_TERM))
00229             _sentence_type = _GPS_SENTENCE_GPGGA;
00230         else if (!gpsstrcmp(_term, _GPGSV_TERM))
00231             _sentence_type = _GPS_SENTENCE_GPGSV;
00232         else
00233             _sentence_type = _GPS_SENTENCE_OTHER;
00234         return false;
00235     }
00236 
00237     if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
00238     /*
00239     if (_sentence_type == _GPS_SENTENCE_GPGSV) {
00240         // $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
00241         // TODO: need to maintain a list of 12 structs with sat info and update it each time
00242         switch (_term_number) {
00243         case 0 : // number of messages
00244             break;
00245         case 1 : // sequence number
00246             break;
00247         case 2 : // satellites in view 
00248             break;
00249         case 3 : // sat ID
00250         case 8 :
00251         case 12 :
00252         case 16 :
00253             break;
00254         case 4 : // elevation
00255         case 9 :
00256         case 13 :
00257         case 17 :
00258             break;
00259         case 5 : // azimuth
00260         case 10 :
00261         case 14 :
00262         case 18 :
00263             break;
00264         case 6 : // SNR
00265         case 11 :
00266         case 15 :
00267         case 19 :
00268             break;
00269         }
00270     } else {*/
00271         switch (((_sentence_type == _GPS_SENTENCE_GPGGA) ? 200 : 100) + _term_number) {
00272         case 101: // Time in both sentences
00273         case 201:
00274           _new_time = parse_decimal();
00275           _new_time_fix = millis();
00276           break;
00277         case 102: // GPRMC validity
00278           _gps_data_good = _term[0] == 'A';
00279           break;
00280         case 103: // Latitude
00281         case 202:
00282           _new_latitude = parse_degrees();
00283           _new_position_fix = millis();
00284           break;
00285         case 104: // N/S
00286         case 203:
00287           if (_term[0] == 'S')
00288             _new_latitude = -_new_latitude;
00289           break;
00290         case 105: // Longitude
00291         case 204:
00292           _new_longitude = parse_degrees();
00293           break;
00294         case 106: // E/W
00295         case 205:
00296           if (_term[0] == 'W')
00297             _new_longitude = -_new_longitude;
00298           break;
00299         case 107: // Speed (GPRMC)
00300           _new_speed = parse_decimal();
00301           break;
00302         case 108: // Course (GPRMC)
00303           _new_course = parse_decimal();
00304           break;
00305         case 109: // Date (GPRMC)
00306           _new_date = gpsatol(_term);
00307           break;
00308         case 206: // Fix data (GPGGA)
00309           _gps_data_good = _term[0] > '0';
00310           break;
00311         case 207: // Number of satelites tracked (GPGGA)
00312           _new_sat_count = parse_int();
00313           break;
00314         case 208: // Horizontal Dilution of Position (GPGGA)
00315           _new_hdop = parse_decimal();
00316           break;
00317         case 209: // Altitude (GPGGA)
00318           _new_altitude = parse_decimal();
00319           break;
00320         default :
00321           break;
00322         } /* switch */
00323     //}
00324 
00325   return false;
00326 }
00327 
00328 long TinyGPS::gpsatol(const char *str)
00329 {
00330   long ret = 0;
00331   while (gpsisdigit(*str))
00332     ret = 10 * ret + *str++ - '0';
00333   return ret;
00334 }
00335 
00336 int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
00337 {
00338   while (*str1 && *str1 == *str2)
00339     ++str1, ++str2;
00340   return *str1;
00341 }