TinyGPS.cpp

00001 /*
00002 TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
00003 Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers.
00004 Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson.
00005 Precision improvements suggested by Wayne Holder.
00006 Copyright (C) 2008-2013 Mikal Hart
00007 All rights reserved.
00008 
00009 This library is free software; you can redistribute it and/or
00010 modify it under the terms of the GNU Lesser General Public
00011 License as published by the Free Software Foundation; either
00012 version 2.1 of the License, or (at your option) any later version.
00013 
00014 This library is distributed in the hope that it will be useful,
00015 but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017 Lesser General Public License for more details.
00018 
00019 You should have received a copy of the GNU Lesser General Public
00020 License along with this library; if not, write to the Free Software
00021 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00022 
00023 MBED port by Sergey Drobyshevskiy (dROb)
00024 */
00025 
00026 #include "TinyGPS.h"
00027 
00028 #define _GPRMC_TERM   "GPRMC"
00029 #define _GPGGA_TERM   "GPGGA"
00030 
00031 TinyGPS::TinyGPS()
00032   :  _time(GPS_INVALID_TIME)
00033   ,  _date(GPS_INVALID_DATE)
00034   ,  _latitude(GPS_INVALID_ANGLE)
00035   ,  _longitude(GPS_INVALID_ANGLE)
00036   ,  _altitude(GPS_INVALID_ALTITUDE)
00037   ,  _speed(GPS_INVALID_SPEED)
00038   ,  _course(GPS_INVALID_ANGLE)
00039   ,  _hdop(GPS_INVALID_HDOP)
00040   ,  _numsats(GPS_INVALID_SATELLITES)
00041   ,  _last_time_fix(GPS_INVALID_FIX_TIME)
00042   ,  _last_position_fix(GPS_INVALID_FIX_TIME)
00043   ,  _parity(0)
00044   ,  _is_checksum_term(false)
00045   ,  _sentence_type(_GPS_SENTENCE_OTHER)
00046   ,  _term_number(0)
00047   ,  _term_offset(0)
00048   ,  _gps_data_good(false)
00049 #ifndef _GPS_NO_STATS
00050   ,  _encoded_characters(0)
00051   ,  _good_sentences(0)
00052   ,  _failed_checksum(0)
00053 #endif
00054 {
00055   _term[0] = '\0';
00056 }
00057 
00058 //
00059 // public methods
00060 //
00061 
00062 bool TinyGPS::encode(char c)
00063 {
00064   bool valid_sentence = false;
00065 
00066 #ifndef _GPS_NO_STATS
00067   ++_encoded_characters;
00068 #endif
00069   switch(c)
00070   {
00071   case ',': // term terminators
00072     _parity ^= c;
00073   case '\r':
00074   case '\n':
00075   case '*':
00076     if (_term_offset < sizeof(_term))
00077     {
00078       _term[_term_offset] = 0;
00079       valid_sentence = term_complete();
00080     }
00081     ++_term_number;
00082     _term_offset = 0;
00083     _is_checksum_term = c == '*';
00084     return valid_sentence;
00085 
00086   case '$': // sentence begin
00087     _term_number = _term_offset = 0;
00088     _parity = 0;
00089     _sentence_type = _GPS_SENTENCE_OTHER;
00090     _is_checksum_term = false;
00091     _gps_data_good = false;
00092     return valid_sentence;
00093   }
00094 
00095   // ordinary characters
00096   if (_term_offset < sizeof(_term) - 1)
00097     _term[_term_offset++] = c;
00098   if (!_is_checksum_term)
00099     _parity ^= c;
00100 
00101   return valid_sentence;
00102 }
00103 
00104 #ifndef _GPS_NO_STATS
00105 void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
00106 {
00107   if (chars) *chars = _encoded_characters;
00108   if (sentences) *sentences = _good_sentences;
00109   if (failed_cs) *failed_cs = _failed_checksum;
00110 }
00111 #endif
00112 
00113 //
00114 // internal utilities
00115 //
00116 int TinyGPS::from_hex(char a) 
00117 {
00118   if (a >= 'A' && a <= 'F')
00119     return a - 'A' + 10;
00120   else if (a >= 'a' && a <= 'f')
00121     return a - 'a' + 10;
00122   else
00123     return a - '0';
00124 }
00125 
00126 unsigned long TinyGPS::parse_decimal()
00127 {
00128   char *p = _term;
00129   bool isneg = *p == '-';
00130   if (isneg) ++p;
00131   unsigned long ret = 100UL * gpsatol(p);
00132   while (gpsisdigit(*p)) ++p;
00133   if (*p == '.')
00134   {
00135     if (gpsisdigit(p[1]))
00136     {
00137       ret += 10 * (p[1] - '0');
00138       if (gpsisdigit(p[2]))
00139         ret += p[2] - '0';
00140     }
00141   }
00142   return isneg ? -ret : ret;
00143 }
00144 
00145 // Parse a string in the form ddmm.mmmmmmm...
00146 unsigned long TinyGPS::parse_degrees()
00147 {
00148   char *p;
00149   unsigned long left_of_decimal = gpsatol(_term);
00150   unsigned long hundred1000ths_of_minute = (left_of_decimal % 100UL) * 100000UL;
00151   for (p=_term; gpsisdigit(*p); ++p);
00152   if (*p == '.')
00153   {
00154     unsigned long mult = 10000;
00155     while (gpsisdigit(*++p))
00156     {
00157       hundred1000ths_of_minute += mult * (*p - '0');
00158       mult /= 10;
00159     }
00160   }
00161   return (left_of_decimal / 100) * 1000000 + (hundred1000ths_of_minute + 3) / 6;
00162 }
00163 
00164 #define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)
00165 
00166 // Processes a just-completed term
00167 // Returns true if new sentence has just passed checksum test and is validated
00168 bool TinyGPS::term_complete()
00169 {
00170   if (_is_checksum_term)
00171   {
00172     byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
00173     if (checksum == _parity)
00174     {
00175       if (_gps_data_good)
00176       {
00177 #ifndef _GPS_NO_STATS
00178         ++_good_sentences;
00179 #endif
00180         _last_time_fix = _new_time_fix;
00181         _last_position_fix = _new_position_fix;
00182 
00183         switch(_sentence_type)
00184         {
00185         case _GPS_SENTENCE_GPRMC:
00186           _time      = _new_time;
00187           _date      = _new_date;
00188           _latitude  = _new_latitude;
00189           _longitude = _new_longitude;
00190           _speed     = _new_speed;
00191           _course    = _new_course;
00192           break;
00193         case _GPS_SENTENCE_GPGGA:
00194           _altitude  = _new_altitude;
00195           _time      = _new_time;
00196           _latitude  = _new_latitude;
00197           _longitude = _new_longitude;
00198           _numsats   = _new_numsats;
00199           _hdop      = _new_hdop;
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
00222       _sentence_type = _GPS_SENTENCE_OTHER;
00223     return false;
00224   }
00225 
00226   if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
00227     switch(COMBINE(_sentence_type, _term_number))
00228   {
00229     case COMBINE(_GPS_SENTENCE_GPRMC, 1): // Time in both sentences
00230     case COMBINE(_GPS_SENTENCE_GPGGA, 1):
00231       _new_time = parse_decimal();
00232       _new_time_fix = millis();
00233       break;
00234     case COMBINE(_GPS_SENTENCE_GPRMC, 2): // GPRMC validity
00235       _gps_data_good = _term[0] == 'A';
00236       break;
00237     case COMBINE(_GPS_SENTENCE_GPRMC, 3): // Latitude
00238     case COMBINE(_GPS_SENTENCE_GPGGA, 2):
00239       _new_latitude = parse_degrees();
00240       _new_position_fix = millis();
00241       break;
00242     case COMBINE(_GPS_SENTENCE_GPRMC, 4): // N/S
00243     case COMBINE(_GPS_SENTENCE_GPGGA, 3):
00244       if (_term[0] == 'S')
00245         _new_latitude = -_new_latitude;
00246       break;
00247     case COMBINE(_GPS_SENTENCE_GPRMC, 5): // Longitude
00248     case COMBINE(_GPS_SENTENCE_GPGGA, 4):
00249       _new_longitude = parse_degrees();
00250       break;
00251     case COMBINE(_GPS_SENTENCE_GPRMC, 6): // E/W
00252     case COMBINE(_GPS_SENTENCE_GPGGA, 5):
00253       if (_term[0] == 'W')
00254         _new_longitude = -_new_longitude;
00255       break;
00256     case COMBINE(_GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC)
00257       _new_speed = parse_decimal();
00258       break;
00259     case COMBINE(_GPS_SENTENCE_GPRMC, 8): // Course (GPRMC)
00260       _new_course = parse_decimal();
00261       break;
00262     case COMBINE(_GPS_SENTENCE_GPRMC, 9): // Date (GPRMC)
00263       _new_date = gpsatol(_term);
00264       break;
00265     case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
00266       _gps_data_good = _term[0] > '0';
00267       break;
00268     case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
00269       _new_numsats = (unsigned char)atoi(_term);
00270       break;
00271     case COMBINE(_GPS_SENTENCE_GPGGA, 8): // HDOP
00272       _new_hdop = parse_decimal();
00273       break;
00274     case COMBINE(_GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA)
00275       _new_altitude = parse_decimal();
00276       break;
00277   }
00278 
00279   return false;
00280 }
00281 
00282 long TinyGPS::gpsatol(const char *str)
00283 {
00284   long ret = 0;
00285   while (gpsisdigit(*str))
00286     ret = 10 * ret + *str++ - '0';
00287   return ret;
00288 }
00289 
00290 int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
00291 {
00292   while (*str1 && *str1 == *str2)
00293     ++str1, ++str2;
00294   return *str1;
00295 }
00296 
00297 /* static */
00298 float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2) 
00299 {
00300   // returns distance in meters between two positions, both specified 
00301   // as signed decimal-degrees latitude and longitude. Uses great-circle 
00302   // distance computation for hypothetical sphere of radius 6372795 meters.
00303   // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
00304   // Courtesy of Maarten Lamers
00305   float delta = toRadians(long1-long2);
00306   float sdlong = sin(delta);
00307   float cdlong = cos(delta);
00308   lat1 = toRadians(lat1);
00309   lat2 = toRadians(lat2);
00310   float slat1 = sin(lat1);
00311   float clat1 = cos(lat1);
00312   float slat2 = sin(lat2);
00313   float clat2 = cos(lat2);
00314   delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); 
00315   delta = delta * delta;//delta = sq(delta); 
00316   delta += (clat2 * sdlong) * (clat2 * sdlong);//delta += sq(clat2 * sdlong); 
00317   delta = sqrt(delta); 
00318   float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); 
00319   delta = atan2(delta, denom); 
00320   return delta * 6372795; 
00321 }
00322 
00323 float TinyGPS::course_to (float lat1, float long1, float lat2, float long2) 
00324 {
00325   // returns course in degrees (North=0, West=270) from position 1 to position 2,
00326   // both specified as signed decimal-degrees latitude and longitude.
00327   // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
00328   // Courtesy of Maarten Lamers
00329   float dlon = toRadians(long2-long1);
00330   lat1 = toRadians(lat1);
00331   lat2 = toRadians(lat2);
00332   float a1 = sin(dlon) * cos(lat2);
00333   float a2 = sin(lat1) * cos(lat2) * cos(dlon);
00334   a2 = cos(lat1) * sin(lat2) - a2;
00335   a2 = atan2(a1, a2);
00336   if (a2 < 0.0)
00337   {
00338     a2 += TWO_PI;
00339   }
00340   return toDegrees(a2);
00341 }
00342 
00343 const char *TinyGPS::cardinal (float course)
00344 {
00345   static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"};
00346 
00347   int direction = (int)((course + 11.25f) / 22.5f);
00348   return directions[direction % 16];
00349 }
00350 
00351 // lat/long in MILLIONTHs of a degree and age of fix in milliseconds
00352 // (note: versions 12 and earlier gave this value in 100,000ths of a degree.
00353 void TinyGPS::get_position(long *latitude, long *longitude, unsigned long *fix_age)
00354 {
00355   if (latitude) *latitude = _latitude;
00356   if (longitude) *longitude = _longitude;
00357   if (fix_age) *fix_age = _last_position_fix == GPS_INVALID_FIX_TIME ? 
00358    GPS_INVALID_AGE : millis() - _last_position_fix;
00359 }
00360 
00361 // date as ddmmyy, time as hhmmsscc, and age in milliseconds
00362 void TinyGPS::get_datetime(unsigned long *date, unsigned long *time, unsigned long *age)
00363 {
00364   if (date) *date = _date;
00365   if (time) *time = _time;
00366   if (age) *age = _last_time_fix == GPS_INVALID_FIX_TIME ? 
00367    GPS_INVALID_AGE : millis() - _last_time_fix;
00368 }
00369 
00370 void TinyGPS::f_get_position(float *latitude, float *longitude, unsigned long *fix_age)
00371 {
00372   long lat, lon;
00373   get_position(&lat, &lon, fix_age);
00374   *latitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lat / 1000000.0);
00375   *longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 1000000.0);
00376 }
00377 
00378 void TinyGPS::crack_datetime(int *year, byte *month, byte *day, 
00379   byte *hour, byte *minute, byte *second, byte *hundredths, unsigned long *age)
00380 {
00381   unsigned long date, time;
00382   get_datetime(&date, &time, age);
00383   if (year) 
00384   {
00385     *year = date % 100;
00386     *year += *year > 80 ? 1900 : 2000;
00387   }
00388   if (month) *month = (date / 100) % 100;
00389   if (day) *day = date / 10000;
00390   if (hour) *hour = time / 1000000;
00391   if (minute) *minute = (time / 10000) % 100;
00392   if (second) *second = (time / 100) % 100;
00393   if (hundredths) *hundredths = time % 100;
00394 }
00395 
00396 float TinyGPS::f_altitude()    
00397 {
00398   return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0;
00399 }
00400 
00401 float TinyGPS::f_course()
00402 {
00403   return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0;
00404 }
00405 
00406 float TinyGPS::f_speed_knots() 
00407 {
00408   return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0;
00409 }
00410 
00411 float TinyGPS::f_speed_mph()   
00412 { 
00413   float sk = f_speed_knots();
00414   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * sk; 
00415 }
00416 
00417 float TinyGPS::f_speed_mps()   
00418 { 
00419   float sk = f_speed_knots();
00420   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * sk; 
00421 }
00422 
00423 float TinyGPS::f_speed_kmph()  
00424 { 
00425   float sk = f_speed_knots();
00426   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * sk; 
00427 }
00428 
00429 const float TinyGPS::GPS_INVALID_F_ANGLE = 1000.0;
00430 const float TinyGPS::GPS_INVALID_F_ALTITUDE = 1000000.0;
00431 const float TinyGPS::GPS_INVALID_F_SPEED = -1.0;