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


lizi_arduino
Author(s): RoboTiCan
autogenerated on Wed Aug 26 2015 12:24:23