TinyGPSPlus.cpp

00001 /*
00002 TinyGPS++ - a small GPS library for Arduino providing universal NMEA parsing
00003 Based on work by and "distanceBetween" and "courseTo" courtesy of Maarten Lamers.
00004 Suggestion to add satellites, courseTo(), and cardinal() by Matt Monson.
00005 Location 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 
00024 #include "TinyGPSPlus.h"
00025 
00026 // Arduino compatibility
00027 #include <mbed.h>
00028 unsigned long millis(void) {
00029     return us_ticker_read() / 1000;
00030 }
00031 
00032 #include <math.h>
00033 #define TWO_PI 6.2831855
00034 double radians(double degrees) {
00035   return (degrees * 71) / 4068;
00036 }
00037 double degrees(double radians) {
00038   return radians * 57296 / 1000;
00039 }
00040 double sq(double n) {
00041   return n*n;
00042 }
00043 // Arduino compatibility
00044 
00045 #include <string.h>
00046 #include <ctype.h>
00047 #include <stdlib.h>
00048 
00049 #define _GPRMCterm   "GPRMC"
00050 #define _GPGGAterm   "GPGGA"
00051 #define _GNRMCterm   "GNRMC"
00052 #define _GNGGAterm   "GNGGA"
00053 
00054 TinyGPSPlus::TinyGPSPlus()
00055   :  parity(0)
00056   ,  isChecksumTerm(false)
00057   ,  curSentenceType(GPS_SENTENCE_OTHER)
00058   ,  curTermNumber(0)
00059   ,  curTermOffset(0)
00060   ,  sentenceHasFix(false)
00061   ,  customElts(0)
00062   ,  customCandidates(0)
00063   ,  encodedCharCount(0)
00064   ,  sentencesWithFixCount(0)
00065   ,  failedChecksumCount(0)
00066   ,  passedChecksumCount(0)
00067 {
00068   term[0] = '\0';
00069 }
00070 
00071 //
00072 // public methods
00073 //
00074 
00075 bool TinyGPSPlus::encode(char c)
00076 {
00077   ++encodedCharCount;
00078 
00079   switch(c)
00080   {
00081   case ',': // term terminators
00082     parity ^= (uint8_t)c;
00083   case '\r':
00084   case '\n':
00085   case '*':
00086     {
00087       bool isValidSentence = false;
00088       if (curTermOffset < sizeof(term))
00089       {
00090         term[curTermOffset] = 0;
00091         isValidSentence = endOfTermHandler();
00092       }
00093       ++curTermNumber;
00094       curTermOffset = 0;
00095       isChecksumTerm = c == '*';
00096       return isValidSentence;
00097     }
00098     break;
00099 
00100   case '$': // sentence begin
00101     curTermNumber = curTermOffset = 0;
00102     parity = 0;
00103     curSentenceType = GPS_SENTENCE_OTHER;
00104     isChecksumTerm = false;
00105     sentenceHasFix = false;
00106     return false;
00107 
00108   default: // ordinary characters
00109     if (curTermOffset < sizeof(term) - 1)
00110       term[curTermOffset++] = c;
00111     if (!isChecksumTerm)
00112       parity ^= c;
00113     return false;
00114   }
00115 
00116   return false;
00117 }
00118 
00119 //
00120 // internal utilities
00121 //
00122 int TinyGPSPlus::fromHex(char a)
00123 {
00124   if (a >= 'A' && a <= 'F')
00125     return a - 'A' + 10;
00126   else if (a >= 'a' && a <= 'f')
00127     return a - 'a' + 10;
00128   else
00129     return a - '0';
00130 }
00131 
00132 // static
00133 // Parse a (potentially negative) number with up to 2 decimal digits -xxxx.yy
00134 int32_t TinyGPSPlus::parseDecimal(const char *term)
00135 {
00136   bool negative = *term == '-';
00137   if (negative) ++term;
00138   int32_t ret = 100 * (int32_t)atol(term);
00139   while (isdigit(*term)) ++term;
00140   if (*term == '.' && isdigit(term[1]))
00141   {
00142     ret += 10 * (term[1] - '0');
00143     if (isdigit(term[2]))
00144       ret += term[2] - '0';
00145   }
00146   return negative ? -ret : ret;
00147 }
00148 
00149 // static
00150 // Parse degrees in that funny NMEA format DDMM.MMMM
00151 void TinyGPSPlus::parseDegrees(const char *term, RawDegrees &deg)
00152 {
00153   uint32_t leftOfDecimal = (uint32_t)atol(term);
00154   uint16_t minutes = (uint16_t)(leftOfDecimal % 100);
00155   uint32_t multiplier = 10000000UL;
00156   uint32_t tenMillionthsOfMinutes = minutes * multiplier;
00157 
00158   deg.deg = (int16_t)(leftOfDecimal / 100);
00159 
00160   while (isdigit(*term))
00161     ++term;
00162 
00163   if (*term == '.')
00164     while (isdigit(*++term))
00165     {
00166       multiplier /= 10;
00167       tenMillionthsOfMinutes += (*term - '0') * multiplier;
00168     }
00169 
00170   deg.billionths = (5 * tenMillionthsOfMinutes + 1) / 3;
00171   deg.negative = false;
00172 }
00173 
00174 #define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)
00175 
00176 // Processes a just-completed term
00177 // Returns true if new sentence has just passed checksum test and is validated
00178 bool TinyGPSPlus::endOfTermHandler()
00179 {
00180   // If it's the checksum term, and the checksum checks out, commit
00181   if (isChecksumTerm)
00182   {
00183     uint8_t checksum = 16 * fromHex(term[0]) + fromHex(term[1]);
00184     if (checksum == parity)
00185     {
00186       passedChecksumCount++;
00187       if (sentenceHasFix)
00188         ++sentencesWithFixCount;
00189 
00190       switch(curSentenceType)
00191       {
00192       case GPS_SENTENCE_GPRMC:
00193         date.commit();
00194         time.commit();
00195         if (sentenceHasFix)
00196         {
00197            location.commit();
00198            speed.commit();
00199            course.commit();
00200         }
00201         break;
00202       case GPS_SENTENCE_GPGGA:
00203         time.commit();
00204         if (sentenceHasFix)
00205         {
00206           location.commit();
00207           altitude.commit();
00208         }
00209         satellites.commit();
00210         hdop.commit();
00211         break;
00212       }
00213 
00214       // Commit all custom listeners of this sentence type
00215       for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp(p->sentenceName, customCandidates->sentenceName) == 0; p = p->next)
00216          p->commit();
00217       return true;
00218     }
00219 
00220     else
00221     {
00222       ++failedChecksumCount;
00223     }
00224 
00225     return false;
00226   }
00227 
00228   // the first term determines the sentence type
00229   if (curTermNumber == 0)
00230   {
00231     if (!strcmp(term, _GPRMCterm) || !strcmp(term, _GNRMCterm))
00232       curSentenceType = GPS_SENTENCE_GPRMC;
00233     else if (!strcmp(term, _GPGGAterm) || !strcmp(term, _GNGGAterm))
00234       curSentenceType = GPS_SENTENCE_GPGGA;
00235     else
00236       curSentenceType = GPS_SENTENCE_OTHER;
00237 
00238     // Any custom candidates of this sentence type?
00239     for (customCandidates = customElts; customCandidates != NULL && strcmp(customCandidates->sentenceName, term) < 0; customCandidates = customCandidates->next);
00240     if (customCandidates != NULL && strcmp(customCandidates->sentenceName, term) > 0)
00241        customCandidates = NULL;
00242 
00243     return false;
00244   }
00245 
00246   if (curSentenceType != GPS_SENTENCE_OTHER && term[0])
00247     switch(COMBINE(curSentenceType, curTermNumber))
00248   {
00249     case COMBINE(GPS_SENTENCE_GPRMC, 1): // Time in both sentences
00250     case COMBINE(GPS_SENTENCE_GPGGA, 1):
00251       time.setTime(term);
00252       break;
00253     case COMBINE(GPS_SENTENCE_GPRMC, 2): // GPRMC validity
00254       sentenceHasFix = term[0] == 'A';
00255       break;
00256     case COMBINE(GPS_SENTENCE_GPRMC, 3): // Latitude
00257     case COMBINE(GPS_SENTENCE_GPGGA, 2):
00258       location.setLatitude(term);
00259       break;
00260     case COMBINE(GPS_SENTENCE_GPRMC, 4): // N/S
00261     case COMBINE(GPS_SENTENCE_GPGGA, 3):
00262       location.rawNewLatData.negative = term[0] == 'S';
00263       break;
00264     case COMBINE(GPS_SENTENCE_GPRMC, 5): // Longitude
00265     case COMBINE(GPS_SENTENCE_GPGGA, 4):
00266       location.setLongitude(term);
00267       break;
00268     case COMBINE(GPS_SENTENCE_GPRMC, 6): // E/W
00269     case COMBINE(GPS_SENTENCE_GPGGA, 5):
00270       location.rawNewLngData.negative = term[0] == 'W';
00271       break;
00272     case COMBINE(GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC)
00273       speed.set(term);
00274       break;
00275     case COMBINE(GPS_SENTENCE_GPRMC, 8): // Course (GPRMC)
00276       course.set(term);
00277       break;
00278     case COMBINE(GPS_SENTENCE_GPRMC, 9): // Date (GPRMC)
00279       date.setDate(term);
00280       break;
00281     case COMBINE(GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
00282       sentenceHasFix = term[0] > '0';
00283       break;
00284     case COMBINE(GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
00285       satellites.set(term);
00286       break;
00287     case COMBINE(GPS_SENTENCE_GPGGA, 8): // HDOP
00288       hdop.set(term);
00289       break;
00290     case COMBINE(GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA)
00291       altitude.set(term);
00292       break;
00293   }
00294 
00295   // Set custom values as needed
00296   for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp(p->sentenceName, customCandidates->sentenceName) == 0 && p->termNumber <= curTermNumber; p = p->next)
00297     if (p->termNumber == curTermNumber)
00298          p->set(term);
00299 
00300   return false;
00301 }
00302 
00303 /* static */
00304 double TinyGPSPlus::distanceBetween(double lat1, double long1, double lat2, double long2)
00305 {
00306   // returns distance in meters between two positions, both specified
00307   // as signed decimal-degrees latitude and longitude. Uses great-circle
00308   // distance computation for hypothetical sphere of radius 6372795 meters.
00309   // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
00310   // Courtesy of Maarten Lamers
00311   double delta = radians(long1-long2);
00312   double sdlong = sin(delta);
00313   double cdlong = cos(delta);
00314   lat1 = radians(lat1);
00315   lat2 = radians(lat2);
00316   double slat1 = sin(lat1);
00317   double clat1 = cos(lat1);
00318   double slat2 = sin(lat2);
00319   double clat2 = cos(lat2);
00320   delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
00321   delta = sq(delta);
00322   delta += sq(clat2 * sdlong);
00323   delta = sqrt(delta);
00324   double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
00325   delta = atan2(delta, denom);
00326   return delta * 6372795;
00327 }
00328 
00329 double TinyGPSPlus::courseTo(double lat1, double long1, double lat2, double long2)
00330 {
00331   // returns course in degrees (North=0, West=270) from position 1 to position 2,
00332   // both specified as signed decimal-degrees latitude and longitude.
00333   // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
00334   // Courtesy of Maarten Lamers
00335   double dlon = radians(long2-long1);
00336   lat1 = radians(lat1);
00337   lat2 = radians(lat2);
00338   double a1 = sin(dlon) * cos(lat2);
00339   double a2 = sin(lat1) * cos(lat2) * cos(dlon);
00340   a2 = cos(lat1) * sin(lat2) - a2;
00341   a2 = atan2(a1, a2);
00342   if (a2 < 0.0)
00343   {
00344     a2 += TWO_PI;
00345   }
00346   return degrees(a2);
00347 }
00348 
00349 const char *TinyGPSPlus::cardinal(double course)
00350 {
00351   static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"};
00352   int direction = (int)((course + 11.25f) / 22.5f);
00353   return directions[direction % 16];
00354 }
00355 
00356 void TinyGPSLocation::commit()
00357 {
00358    rawLatData = rawNewLatData;
00359    rawLngData = rawNewLngData;
00360    lastCommitTime = millis();
00361    valid = updated = true;
00362 }
00363 
00364 void TinyGPSLocation::setLatitude(const char *term)
00365 {
00366    TinyGPSPlus::parseDegrees(term, rawNewLatData);
00367 }
00368 
00369 void TinyGPSLocation::setLongitude(const char *term)
00370 {
00371    TinyGPSPlus::parseDegrees(term, rawNewLngData);
00372 }
00373 
00374 double TinyGPSLocation::lat()
00375 {
00376    updated = false;
00377    double ret = rawLatData.deg + rawLatData.billionths / 1000000000.0;
00378    return rawLatData.negative ? -ret : ret;
00379 }
00380 
00381 double TinyGPSLocation::lng()
00382 {
00383    updated = false;
00384    double ret = rawLngData.deg + rawLngData.billionths / 1000000000.0;
00385    return rawLngData.negative ? -ret : ret;
00386 }
00387 
00388 void TinyGPSDate::commit()
00389 {
00390    date = newDate;
00391    lastCommitTime = millis();
00392    valid = updated = true;
00393 }
00394 
00395 void TinyGPSTime::commit()
00396 {
00397    time = newTime;
00398    lastCommitTime = millis();
00399    valid = updated = true;
00400 }
00401 
00402 void TinyGPSTime::setTime(const char *term)
00403 {
00404    newTime = (uint32_t)TinyGPSPlus::parseDecimal(term);
00405 }
00406 
00407 void TinyGPSDate::setDate(const char *term)
00408 {
00409    newDate = atol(term);
00410 }
00411 
00412 uint16_t TinyGPSDate::year()
00413 {
00414    updated = false;
00415    uint16_t year = date % 100;
00416    return year + 2000;
00417 }
00418 
00419 uint8_t TinyGPSDate::month()
00420 {
00421    updated = false;
00422    return (date / 100) % 100;
00423 }
00424 
00425 uint8_t TinyGPSDate::day()
00426 {
00427    updated = false;
00428    return date / 10000;
00429 }
00430 
00431 uint8_t TinyGPSTime::hour()
00432 {
00433    updated = false;
00434    return time / 1000000;
00435 }
00436 
00437 uint8_t TinyGPSTime::minute()
00438 {
00439    updated = false;
00440    return (time / 10000) % 100;
00441 }
00442 
00443 uint8_t TinyGPSTime::second()
00444 {
00445    updated = false;
00446    return (time / 100) % 100;
00447 }
00448 
00449 uint8_t TinyGPSTime::centisecond()
00450 {
00451    updated = false;
00452    return time % 100;
00453 }
00454 
00455 void TinyGPSDecimal::commit()
00456 {
00457    val = newval;
00458    lastCommitTime = millis();
00459    valid = updated = true;
00460 }
00461 
00462 void TinyGPSDecimal::set(const char *term)
00463 {
00464    newval = TinyGPSPlus::parseDecimal(term);
00465 }
00466 
00467 void TinyGPSInteger::commit()
00468 {
00469    val = newval;
00470    lastCommitTime = millis();
00471    valid = updated = true;
00472 }
00473 
00474 void TinyGPSInteger::set(const char *term)
00475 {
00476    newval = atol(term);
00477 }
00478 
00479 TinyGPSCustom::TinyGPSCustom(TinyGPSPlus &gps, const char *_sentenceName, int _termNumber)
00480 {
00481    begin(gps, _sentenceName, _termNumber);
00482 }
00483 
00484 void TinyGPSCustom::begin(TinyGPSPlus &gps, const char *_sentenceName, int _termNumber)
00485 {
00486    lastCommitTime = 0;
00487    updated = valid = false;
00488    sentenceName = _sentenceName;
00489    termNumber = _termNumber;
00490    memset(stagingBuffer, '\0', sizeof(stagingBuffer));
00491    memset(buffer, '\0', sizeof(buffer));
00492 
00493    // Insert this item into the GPS tree
00494    gps.insertCustom(this, _sentenceName, _termNumber);
00495 }
00496 
00497 void TinyGPSCustom::commit()
00498 {
00499    strcpy(this->buffer, this->stagingBuffer);
00500    lastCommitTime = millis();
00501    valid = updated = true;
00502 }
00503 
00504 void TinyGPSCustom::set(const char *term)
00505 {
00506    strncpy(this->stagingBuffer, term, sizeof(this->stagingBuffer));
00507 }
00508 
00509 void TinyGPSPlus::insertCustom(TinyGPSCustom *pElt, const char *sentenceName, int termNumber)
00510 {
00511    TinyGPSCustom **ppelt;
00512 
00513    for (ppelt = &this->customElts; *ppelt != NULL; ppelt = &(*ppelt)->next)
00514    {
00515       int cmp = strcmp(sentenceName, (*ppelt)->sentenceName);
00516       if (cmp < 0 || (cmp == 0 && termNumber < (*ppelt)->termNumber))
00517          break;
00518    }
00519 
00520    pElt->next = *ppelt;
00521    *ppelt = pElt;
00522 }