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