SilentSensors / TinyGPSPlus

mbed port of the TinyGPSPlus library for Arduino, by Mikal Hart. The port is heavily inspired by Sergey Drobyshevskiy (dROb)'s port of the TinyGPS library.

Dependents:   ZZ_SSL_Main_L476 Sample_program_Font72

Fork of TinyGPSPlus by カレヴィ アンダース

TinyGPSPlus.cpp@0:2576f9f1dc71, 2016-09-14 (annotated)

Committer:
OkiAviation
Date:
Wed Sep 14 01:38:28 2016 +0000
Revision:
0:2576f9f1dc71
Child:
1:a4d6efebcef8
Ported the TinyGPSPlus Library (originally by Mikal Hart) to mbed. The Port is heavily inspired by Sergey Drobyshevskiy (dROb), wo ported the TinyGPS library to mbed.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OkiAviation 0:2576f9f1dc71 1 /*
OkiAviation 0:2576f9f1dc71 2 TinyGPS++ - a small GPS library for Arduino providing universal NMEA parsing
OkiAviation 0:2576f9f1dc71 3 Based on work by and "distanceBetween" and "courseTo" courtesy of Maarten Lamers.
OkiAviation 0:2576f9f1dc71 4 Suggestion to add satellites, courseTo(), and cardinal() by Matt Monson.
OkiAviation 0:2576f9f1dc71 5 Location precision improvements suggested by Wayne Holder.
OkiAviation 0:2576f9f1dc71 6 Copyright (C) 2008-2013 Mikal Hart
OkiAviation 0:2576f9f1dc71 7 All rights reserved.
OkiAviation 0:2576f9f1dc71 8
OkiAviation 0:2576f9f1dc71 9 This library is free software; you can redistribute it and/or
OkiAviation 0:2576f9f1dc71 10 modify it under the terms of the GNU Lesser General Public
OkiAviation 0:2576f9f1dc71 11 License as published by the Free Software Foundation; either
OkiAviation 0:2576f9f1dc71 12 version 2.1 of the License, or (at your option) any later version.
OkiAviation 0:2576f9f1dc71 13
OkiAviation 0:2576f9f1dc71 14 This library is distributed in the hope that it will be useful,
OkiAviation 0:2576f9f1dc71 15 but WITHOUT ANY WARRANTY; without even the implied warranty of
OkiAviation 0:2576f9f1dc71 16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
OkiAviation 0:2576f9f1dc71 17 Lesser General Public License for more details.
OkiAviation 0:2576f9f1dc71 18
OkiAviation 0:2576f9f1dc71 19 You should have received a copy of the GNU Lesser General Public
OkiAviation 0:2576f9f1dc71 20 License along with this library; if not, write to the Free Software
OkiAviation 0:2576f9f1dc71 21 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
OkiAviation 0:2576f9f1dc71 22
OkiAviation 0:2576f9f1dc71 23 MBED port by Kalevi Anders, but heavily inspired by Sergey Drobyshevskiy (dROb)
OkiAviation 0:2576f9f1dc71 24 */
OkiAviation 0:2576f9f1dc71 25
OkiAviation 0:2576f9f1dc71 26 #include "TinyGPSPlus.h"
OkiAviation 0:2576f9f1dc71 27
OkiAviation 0:2576f9f1dc71 28 #include <string.h>
OkiAviation 0:2576f9f1dc71 29 #include <ctype.h>
OkiAviation 0:2576f9f1dc71 30 #include <stdlib.h>
OkiAviation 0:2576f9f1dc71 31
OkiAviation 0:2576f9f1dc71 32 #define _GPRMCterm "GPRMC"
OkiAviation 0:2576f9f1dc71 33 #define _GPGGAterm "GPGGA"
OkiAviation 0:2576f9f1dc71 34
OkiAviation 0:2576f9f1dc71 35 TinyGPSPlus::TinyGPSPlus()
OkiAviation 0:2576f9f1dc71 36 : parity(0)
OkiAviation 0:2576f9f1dc71 37 , isChecksumTerm(false)
OkiAviation 0:2576f9f1dc71 38 , curSentenceType(GPS_SENTENCE_OTHER)
OkiAviation 0:2576f9f1dc71 39 , curTermNumber(0)
OkiAviation 0:2576f9f1dc71 40 , curTermOffset(0)
OkiAviation 0:2576f9f1dc71 41 , sentenceHasFix(false)
OkiAviation 0:2576f9f1dc71 42 , customElts(0)
OkiAviation 0:2576f9f1dc71 43 , customCandidates(0)
OkiAviation 0:2576f9f1dc71 44 , encodedCharCount(0)
OkiAviation 0:2576f9f1dc71 45 , sentencesWithFixCount(0)
OkiAviation 0:2576f9f1dc71 46 , failedChecksumCount(0)
OkiAviation 0:2576f9f1dc71 47 , passedChecksumCount(0)
OkiAviation 0:2576f9f1dc71 48 {
OkiAviation 0:2576f9f1dc71 49 term[0] = '\0';
OkiAviation 0:2576f9f1dc71 50 }
OkiAviation 0:2576f9f1dc71 51
OkiAviation 0:2576f9f1dc71 52 //
OkiAviation 0:2576f9f1dc71 53 // public methods
OkiAviation 0:2576f9f1dc71 54 //
OkiAviation 0:2576f9f1dc71 55
OkiAviation 0:2576f9f1dc71 56 bool TinyGPSPlus::encode(char c)
OkiAviation 0:2576f9f1dc71 57 {
OkiAviation 0:2576f9f1dc71 58 ++encodedCharCount;
OkiAviation 0:2576f9f1dc71 59
OkiAviation 0:2576f9f1dc71 60
OkiAviation 0:2576f9f1dc71 61 switch(c)
OkiAviation 0:2576f9f1dc71 62 {
OkiAviation 0:2576f9f1dc71 63 case ',': // term terminators
OkiAviation 0:2576f9f1dc71 64 parity ^= (uint8_t)c;
OkiAviation 0:2576f9f1dc71 65 case '\r':
OkiAviation 0:2576f9f1dc71 66 case '\n':
OkiAviation 0:2576f9f1dc71 67 case '*':
OkiAviation 0:2576f9f1dc71 68 {
OkiAviation 0:2576f9f1dc71 69 bool isValidSentence = false;
OkiAviation 0:2576f9f1dc71 70 if (curTermOffset < sizeof(term))
OkiAviation 0:2576f9f1dc71 71 {
OkiAviation 0:2576f9f1dc71 72 term[curTermOffset] = 0;
OkiAviation 0:2576f9f1dc71 73 isValidSentence = endOfTermHandler();
OkiAviation 0:2576f9f1dc71 74 }
OkiAviation 0:2576f9f1dc71 75 ++curTermNumber;
OkiAviation 0:2576f9f1dc71 76 curTermOffset = 0;
OkiAviation 0:2576f9f1dc71 77 isChecksumTerm = c == '*';
OkiAviation 0:2576f9f1dc71 78 return isValidSentence;
OkiAviation 0:2576f9f1dc71 79 }
OkiAviation 0:2576f9f1dc71 80 break;
OkiAviation 0:2576f9f1dc71 81
OkiAviation 0:2576f9f1dc71 82 case '$': // sentence begin
OkiAviation 0:2576f9f1dc71 83 curTermNumber = curTermOffset = 0;
OkiAviation 0:2576f9f1dc71 84 parity = 0;
OkiAviation 0:2576f9f1dc71 85 curSentenceType = GPS_SENTENCE_OTHER;
OkiAviation 0:2576f9f1dc71 86 isChecksumTerm = false;
OkiAviation 0:2576f9f1dc71 87 sentenceHasFix = false;
OkiAviation 0:2576f9f1dc71 88 return false;
OkiAviation 0:2576f9f1dc71 89
OkiAviation 0:2576f9f1dc71 90 default: // ordinary characters
OkiAviation 0:2576f9f1dc71 91 if (curTermOffset < sizeof(term) - 1)
OkiAviation 0:2576f9f1dc71 92 term[curTermOffset++] = c;
OkiAviation 0:2576f9f1dc71 93 if (!isChecksumTerm)
OkiAviation 0:2576f9f1dc71 94 parity ^= c;
OkiAviation 0:2576f9f1dc71 95 return false;
OkiAviation 0:2576f9f1dc71 96 }
OkiAviation 0:2576f9f1dc71 97
OkiAviation 0:2576f9f1dc71 98 return false;
OkiAviation 0:2576f9f1dc71 99 }
OkiAviation 0:2576f9f1dc71 100
OkiAviation 0:2576f9f1dc71 101 //
OkiAviation 0:2576f9f1dc71 102 // internal utilities
OkiAviation 0:2576f9f1dc71 103 //
OkiAviation 0:2576f9f1dc71 104 int TinyGPSPlus::fromHex(char a)
OkiAviation 0:2576f9f1dc71 105 {
OkiAviation 0:2576f9f1dc71 106 if (a >= 'A' && a <= 'F')
OkiAviation 0:2576f9f1dc71 107 return a - 'A' + 10;
OkiAviation 0:2576f9f1dc71 108 else if (a >= 'a' && a <= 'f')
OkiAviation 0:2576f9f1dc71 109 return a - 'a' + 10;
OkiAviation 0:2576f9f1dc71 110 else
OkiAviation 0:2576f9f1dc71 111 return a - '0';
OkiAviation 0:2576f9f1dc71 112 }
OkiAviation 0:2576f9f1dc71 113
OkiAviation 0:2576f9f1dc71 114 // static
OkiAviation 0:2576f9f1dc71 115 // Parse a (potentially negative) number with up to 2 decimal digits -xxxx.yy
OkiAviation 0:2576f9f1dc71 116 int32_t TinyGPSPlus::parseDecimal(const char *term)
OkiAviation 0:2576f9f1dc71 117 {
OkiAviation 0:2576f9f1dc71 118 bool negative = *term == '-';
OkiAviation 0:2576f9f1dc71 119 if (negative) ++term;
OkiAviation 0:2576f9f1dc71 120 int32_t ret = 100 * (int32_t)atol(term);
OkiAviation 0:2576f9f1dc71 121 while (isdigit(*term)) ++term;
OkiAviation 0:2576f9f1dc71 122 if (*term == '.' && isdigit(term[1]))
OkiAviation 0:2576f9f1dc71 123 {
OkiAviation 0:2576f9f1dc71 124 ret += 10 * (term[1] - '0');
OkiAviation 0:2576f9f1dc71 125 if (isdigit(term[2]))
OkiAviation 0:2576f9f1dc71 126 ret += term[2] - '0';
OkiAviation 0:2576f9f1dc71 127 }
OkiAviation 0:2576f9f1dc71 128 return negative ? -ret : ret;
OkiAviation 0:2576f9f1dc71 129 }
OkiAviation 0:2576f9f1dc71 130
OkiAviation 0:2576f9f1dc71 131 // static
OkiAviation 0:2576f9f1dc71 132 // Parse degrees in that funny NMEA format DDMM.MMMM
OkiAviation 0:2576f9f1dc71 133 void TinyGPSPlus::parseDegrees(const char *term, RawDegrees &deg)
OkiAviation 0:2576f9f1dc71 134 {
OkiAviation 0:2576f9f1dc71 135 uint32_t leftOfDecimal = (uint32_t)atol(term);
OkiAviation 0:2576f9f1dc71 136 uint16_t minutes = (uint16_t)(leftOfDecimal % 100);
OkiAviation 0:2576f9f1dc71 137 uint32_t multiplier = 10000000UL;
OkiAviation 0:2576f9f1dc71 138 uint32_t tenMillionthsOfMinutes = minutes * multiplier;
OkiAviation 0:2576f9f1dc71 139
OkiAviation 0:2576f9f1dc71 140 deg.deg = (int16_t)(leftOfDecimal / 100);
OkiAviation 0:2576f9f1dc71 141
OkiAviation 0:2576f9f1dc71 142 while (isdigit(*term))
OkiAviation 0:2576f9f1dc71 143 ++term;
OkiAviation 0:2576f9f1dc71 144
OkiAviation 0:2576f9f1dc71 145 if (*term == '.')
OkiAviation 0:2576f9f1dc71 146 while (isdigit(*++term))
OkiAviation 0:2576f9f1dc71 147 {
OkiAviation 0:2576f9f1dc71 148 multiplier /= 10;
OkiAviation 0:2576f9f1dc71 149 tenMillionthsOfMinutes += (*term - '0') * multiplier;
OkiAviation 0:2576f9f1dc71 150 }
OkiAviation 0:2576f9f1dc71 151
OkiAviation 0:2576f9f1dc71 152 deg.billionths = (5 * tenMillionthsOfMinutes + 1) / 3;
OkiAviation 0:2576f9f1dc71 153 deg.negative = false;
OkiAviation 0:2576f9f1dc71 154 }
OkiAviation 0:2576f9f1dc71 155
OkiAviation 0:2576f9f1dc71 156 #define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)
OkiAviation 0:2576f9f1dc71 157
OkiAviation 0:2576f9f1dc71 158 // Processes a just-completed term
OkiAviation 0:2576f9f1dc71 159 // Returns true if new sentence has just passed checksum test and is validated
OkiAviation 0:2576f9f1dc71 160 bool TinyGPSPlus::endOfTermHandler()
OkiAviation 0:2576f9f1dc71 161 {
OkiAviation 0:2576f9f1dc71 162 // If it's the checksum term, and the checksum checks out, commit
OkiAviation 0:2576f9f1dc71 163 if (isChecksumTerm)
OkiAviation 0:2576f9f1dc71 164 {
OkiAviation 0:2576f9f1dc71 165 byte checksum = 16 * fromHex(term[0]) + fromHex(term[1]);
OkiAviation 0:2576f9f1dc71 166 if (checksum == parity)
OkiAviation 0:2576f9f1dc71 167 {
OkiAviation 0:2576f9f1dc71 168 passedChecksumCount++;
OkiAviation 0:2576f9f1dc71 169 if (sentenceHasFix)
OkiAviation 0:2576f9f1dc71 170 ++sentencesWithFixCount;
OkiAviation 0:2576f9f1dc71 171
OkiAviation 0:2576f9f1dc71 172 switch(curSentenceType)
OkiAviation 0:2576f9f1dc71 173 {
OkiAviation 0:2576f9f1dc71 174 case GPS_SENTENCE_GPRMC:
OkiAviation 0:2576f9f1dc71 175 date.commit();
OkiAviation 0:2576f9f1dc71 176 time.commit();
OkiAviation 0:2576f9f1dc71 177 if (sentenceHasFix)
OkiAviation 0:2576f9f1dc71 178 {
OkiAviation 0:2576f9f1dc71 179 location.commit();
OkiAviation 0:2576f9f1dc71 180 speed.commit();
OkiAviation 0:2576f9f1dc71 181 course.commit();
OkiAviation 0:2576f9f1dc71 182 }
OkiAviation 0:2576f9f1dc71 183 break;
OkiAviation 0:2576f9f1dc71 184 case GPS_SENTENCE_GPGGA:
OkiAviation 0:2576f9f1dc71 185 time.commit();
OkiAviation 0:2576f9f1dc71 186 if (sentenceHasFix)
OkiAviation 0:2576f9f1dc71 187 {
OkiAviation 0:2576f9f1dc71 188 location.commit();
OkiAviation 0:2576f9f1dc71 189 altitude.commit();
OkiAviation 0:2576f9f1dc71 190 }
OkiAviation 0:2576f9f1dc71 191 satellites.commit();
OkiAviation 0:2576f9f1dc71 192 hdop.commit();
OkiAviation 0:2576f9f1dc71 193 break;
OkiAviation 0:2576f9f1dc71 194 }
OkiAviation 0:2576f9f1dc71 195
OkiAviation 0:2576f9f1dc71 196 // Commit all custom listeners of this sentence type
OkiAviation 0:2576f9f1dc71 197 for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp(p->sentenceName, customCandidates->sentenceName) == 0; p = p->next)
OkiAviation 0:2576f9f1dc71 198 p->commit();
OkiAviation 0:2576f9f1dc71 199 return true;
OkiAviation 0:2576f9f1dc71 200 }
OkiAviation 0:2576f9f1dc71 201
OkiAviation 0:2576f9f1dc71 202 else
OkiAviation 0:2576f9f1dc71 203 {
OkiAviation 0:2576f9f1dc71 204 ++failedChecksumCount;
OkiAviation 0:2576f9f1dc71 205 }
OkiAviation 0:2576f9f1dc71 206
OkiAviation 0:2576f9f1dc71 207 return false;
OkiAviation 0:2576f9f1dc71 208 }
OkiAviation 0:2576f9f1dc71 209
OkiAviation 0:2576f9f1dc71 210 // the first term determines the sentence type
OkiAviation 0:2576f9f1dc71 211 if (curTermNumber == 0)
OkiAviation 0:2576f9f1dc71 212 {
OkiAviation 0:2576f9f1dc71 213 if (!strcmp(term, _GPRMCterm))
OkiAviation 0:2576f9f1dc71 214 curSentenceType = GPS_SENTENCE_GPRMC;
OkiAviation 0:2576f9f1dc71 215 else if (!strcmp(term, _GPGGAterm))
OkiAviation 0:2576f9f1dc71 216 curSentenceType = GPS_SENTENCE_GPGGA;
OkiAviation 0:2576f9f1dc71 217 else
OkiAviation 0:2576f9f1dc71 218 curSentenceType = GPS_SENTENCE_OTHER;
OkiAviation 0:2576f9f1dc71 219
OkiAviation 0:2576f9f1dc71 220 // Any custom candidates of this sentence type?
OkiAviation 0:2576f9f1dc71 221 for (customCandidates = customElts; customCandidates != NULL && strcmp(customCandidates->sentenceName, term) < 0; customCandidates = customCandidates->next);
OkiAviation 0:2576f9f1dc71 222 if (customCandidates != NULL && strcmp(customCandidates->sentenceName, term) > 0)
OkiAviation 0:2576f9f1dc71 223 customCandidates = NULL;
OkiAviation 0:2576f9f1dc71 224
OkiAviation 0:2576f9f1dc71 225 return false;
OkiAviation 0:2576f9f1dc71 226 }
OkiAviation 0:2576f9f1dc71 227
OkiAviation 0:2576f9f1dc71 228 if (curSentenceType != GPS_SENTENCE_OTHER && term[0])
OkiAviation 0:2576f9f1dc71 229 switch(COMBINE(curSentenceType, curTermNumber))
OkiAviation 0:2576f9f1dc71 230 {
OkiAviation 0:2576f9f1dc71 231 case COMBINE(GPS_SENTENCE_GPRMC, 1): // Time in both sentences
OkiAviation 0:2576f9f1dc71 232 case COMBINE(GPS_SENTENCE_GPGGA, 1):
OkiAviation 0:2576f9f1dc71 233 time.setTime(term);
OkiAviation 0:2576f9f1dc71 234 break;
OkiAviation 0:2576f9f1dc71 235 case COMBINE(GPS_SENTENCE_GPRMC, 2): // GPRMC validity
OkiAviation 0:2576f9f1dc71 236 sentenceHasFix = term[0] == 'A';
OkiAviation 0:2576f9f1dc71 237 break;
OkiAviation 0:2576f9f1dc71 238 case COMBINE(GPS_SENTENCE_GPRMC, 3): // Latitude
OkiAviation 0:2576f9f1dc71 239 case COMBINE(GPS_SENTENCE_GPGGA, 2):
OkiAviation 0:2576f9f1dc71 240 location.setLatitude(term);
OkiAviation 0:2576f9f1dc71 241 break;
OkiAviation 0:2576f9f1dc71 242 case COMBINE(GPS_SENTENCE_GPRMC, 4): // N/S
OkiAviation 0:2576f9f1dc71 243 case COMBINE(GPS_SENTENCE_GPGGA, 3):
OkiAviation 0:2576f9f1dc71 244 location.rawNewLatData.negative = term[0] == 'S';
OkiAviation 0:2576f9f1dc71 245 break;
OkiAviation 0:2576f9f1dc71 246 case COMBINE(GPS_SENTENCE_GPRMC, 5): // Longitude
OkiAviation 0:2576f9f1dc71 247 case COMBINE(GPS_SENTENCE_GPGGA, 4):
OkiAviation 0:2576f9f1dc71 248 location.setLongitude(term);
OkiAviation 0:2576f9f1dc71 249 break;
OkiAviation 0:2576f9f1dc71 250 case COMBINE(GPS_SENTENCE_GPRMC, 6): // E/W
OkiAviation 0:2576f9f1dc71 251 case COMBINE(GPS_SENTENCE_GPGGA, 5):
OkiAviation 0:2576f9f1dc71 252 location.rawNewLngData.negative = term[0] == 'W';
OkiAviation 0:2576f9f1dc71 253 break;
OkiAviation 0:2576f9f1dc71 254 case COMBINE(GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC)
OkiAviation 0:2576f9f1dc71 255 speed.set(term);
OkiAviation 0:2576f9f1dc71 256 break;
OkiAviation 0:2576f9f1dc71 257 case COMBINE(GPS_SENTENCE_GPRMC, 8): // Course (GPRMC)
OkiAviation 0:2576f9f1dc71 258 course.set(term);
OkiAviation 0:2576f9f1dc71 259 break;
OkiAviation 0:2576f9f1dc71 260 case COMBINE(GPS_SENTENCE_GPRMC, 9): // Date (GPRMC)
OkiAviation 0:2576f9f1dc71 261 date.setDate(term);
OkiAviation 0:2576f9f1dc71 262 break;
OkiAviation 0:2576f9f1dc71 263 case COMBINE(GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
OkiAviation 0:2576f9f1dc71 264 sentenceHasFix = term[0] > '0';
OkiAviation 0:2576f9f1dc71 265 break;
OkiAviation 0:2576f9f1dc71 266 case COMBINE(GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
OkiAviation 0:2576f9f1dc71 267 satellites.set(term);
OkiAviation 0:2576f9f1dc71 268 break;
OkiAviation 0:2576f9f1dc71 269 case COMBINE(GPS_SENTENCE_GPGGA, 8): // HDOP
OkiAviation 0:2576f9f1dc71 270 hdop.set(term);
OkiAviation 0:2576f9f1dc71 271 break;
OkiAviation 0:2576f9f1dc71 272 case COMBINE(GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA)
OkiAviation 0:2576f9f1dc71 273 altitude.set(term);
OkiAviation 0:2576f9f1dc71 274 break;
OkiAviation 0:2576f9f1dc71 275 }
OkiAviation 0:2576f9f1dc71 276
OkiAviation 0:2576f9f1dc71 277 // Set custom values as needed
OkiAviation 0:2576f9f1dc71 278 for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp(p->sentenceName, customCandidates->sentenceName) == 0 && p->termNumber <= curTermNumber; p = p->next)
OkiAviation 0:2576f9f1dc71 279 if (p->termNumber == curTermNumber)
OkiAviation 0:2576f9f1dc71 280 p->set(term);
OkiAviation 0:2576f9f1dc71 281
OkiAviation 0:2576f9f1dc71 282 return false;
OkiAviation 0:2576f9f1dc71 283 }
OkiAviation 0:2576f9f1dc71 284
OkiAviation 0:2576f9f1dc71 285 /* static */
OkiAviation 0:2576f9f1dc71 286 double TinyGPSPlus::distanceBetween(double lat1, double long1, double lat2, double long2)
OkiAviation 0:2576f9f1dc71 287 {
OkiAviation 0:2576f9f1dc71 288 // returns distance in meters between two positions, both specified
OkiAviation 0:2576f9f1dc71 289 // as signed decimal-degrees latitude and longitude. Uses great-circle
OkiAviation 0:2576f9f1dc71 290 // distance computation for hypothetical sphere of radius 6372795 meters.
OkiAviation 0:2576f9f1dc71 291 // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
OkiAviation 0:2576f9f1dc71 292 // Courtesy of Maarten Lamers
OkiAviation 0:2576f9f1dc71 293 double delta = radians(long1-long2);
OkiAviation 0:2576f9f1dc71 294 double sdlong = sin(delta);
OkiAviation 0:2576f9f1dc71 295 double cdlong = cos(delta);
OkiAviation 0:2576f9f1dc71 296 lat1 = radians(lat1);
OkiAviation 0:2576f9f1dc71 297 lat2 = radians(lat2);
OkiAviation 0:2576f9f1dc71 298 double slat1 = sin(lat1);
OkiAviation 0:2576f9f1dc71 299 double clat1 = cos(lat1);
OkiAviation 0:2576f9f1dc71 300 double slat2 = sin(lat2);
OkiAviation 0:2576f9f1dc71 301 double clat2 = cos(lat2);
OkiAviation 0:2576f9f1dc71 302 delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
OkiAviation 0:2576f9f1dc71 303 delta = sq(delta);
OkiAviation 0:2576f9f1dc71 304 delta += sq(clat2 * sdlong);
OkiAviation 0:2576f9f1dc71 305 delta = sqrt(delta);
OkiAviation 0:2576f9f1dc71 306 double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
OkiAviation 0:2576f9f1dc71 307 delta = atan2(delta, denom);
OkiAviation 0:2576f9f1dc71 308 return delta * 6372795;
OkiAviation 0:2576f9f1dc71 309 }
OkiAviation 0:2576f9f1dc71 310
OkiAviation 0:2576f9f1dc71 311 double TinyGPSPlus::courseTo(double lat1, double long1, double lat2, double long2)
OkiAviation 0:2576f9f1dc71 312 {
OkiAviation 0:2576f9f1dc71 313 // returns course in degrees (North=0, West=270) from position 1 to position 2,
OkiAviation 0:2576f9f1dc71 314 // both specified as signed decimal-degrees latitude and longitude.
OkiAviation 0:2576f9f1dc71 315 // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
OkiAviation 0:2576f9f1dc71 316 // Courtesy of Maarten Lamers
OkiAviation 0:2576f9f1dc71 317 double dlon = radians(long2-long1);
OkiAviation 0:2576f9f1dc71 318 lat1 = radians(lat1);
OkiAviation 0:2576f9f1dc71 319 lat2 = radians(lat2);
OkiAviation 0:2576f9f1dc71 320 double a1 = sin(dlon) * cos(lat2);
OkiAviation 0:2576f9f1dc71 321 double a2 = sin(lat1) * cos(lat2) * cos(dlon);
OkiAviation 0:2576f9f1dc71 322 a2 = cos(lat1) * sin(lat2) - a2;
OkiAviation 0:2576f9f1dc71 323 a2 = atan2(a1, a2);
OkiAviation 0:2576f9f1dc71 324 if (a2 < 0.0)
OkiAviation 0:2576f9f1dc71 325 {
OkiAviation 0:2576f9f1dc71 326 a2 += TWO_PI;
OkiAviation 0:2576f9f1dc71 327 }
OkiAviation 0:2576f9f1dc71 328 return degrees(a2);
OkiAviation 0:2576f9f1dc71 329 }
OkiAviation 0:2576f9f1dc71 330
OkiAviation 0:2576f9f1dc71 331 const char *TinyGPSPlus::cardinal(double course)
OkiAviation 0:2576f9f1dc71 332 {
OkiAviation 0:2576f9f1dc71 333 static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"};
OkiAviation 0:2576f9f1dc71 334 int direction = (int)((course + 11.25f) / 22.5f);
OkiAviation 0:2576f9f1dc71 335 return directions[direction % 16];
OkiAviation 0:2576f9f1dc71 336 }
OkiAviation 0:2576f9f1dc71 337
OkiAviation 0:2576f9f1dc71 338 void TinyGPSLocation::commit()
OkiAviation 0:2576f9f1dc71 339 {
OkiAviation 0:2576f9f1dc71 340 rawLatData = rawNewLatData;
OkiAviation 0:2576f9f1dc71 341 rawLngData = rawNewLngData;
OkiAviation 0:2576f9f1dc71 342 lastCommitTime = millis();
OkiAviation 0:2576f9f1dc71 343 valid = updated = true;
OkiAviation 0:2576f9f1dc71 344 }
OkiAviation 0:2576f9f1dc71 345
OkiAviation 0:2576f9f1dc71 346 void TinyGPSLocation::setLatitude(const char *term)
OkiAviation 0:2576f9f1dc71 347 {
OkiAviation 0:2576f9f1dc71 348 TinyGPSPlus::parseDegrees(term, rawNewLatData);
OkiAviation 0:2576f9f1dc71 349 }
OkiAviation 0:2576f9f1dc71 350
OkiAviation 0:2576f9f1dc71 351 void TinyGPSLocation::setLongitude(const char *term)
OkiAviation 0:2576f9f1dc71 352 {
OkiAviation 0:2576f9f1dc71 353 TinyGPSPlus::parseDegrees(term, rawNewLngData);
OkiAviation 0:2576f9f1dc71 354 }
OkiAviation 0:2576f9f1dc71 355
OkiAviation 0:2576f9f1dc71 356 double TinyGPSLocation::lat()
OkiAviation 0:2576f9f1dc71 357 {
OkiAviation 0:2576f9f1dc71 358 updated = false;
OkiAviation 0:2576f9f1dc71 359 double ret = rawLatData.deg + rawLatData.billionths / 1000000000.0;
OkiAviation 0:2576f9f1dc71 360 return rawLatData.negative ? -ret : ret;
OkiAviation 0:2576f9f1dc71 361 }
OkiAviation 0:2576f9f1dc71 362
OkiAviation 0:2576f9f1dc71 363 double TinyGPSLocation::lng()
OkiAviation 0:2576f9f1dc71 364 {
OkiAviation 0:2576f9f1dc71 365 updated = false;
OkiAviation 0:2576f9f1dc71 366 double ret = rawLngData.deg + rawLngData.billionths / 1000000000.0;
OkiAviation 0:2576f9f1dc71 367 return rawLngData.negative ? -ret : ret;
OkiAviation 0:2576f9f1dc71 368 }
OkiAviation 0:2576f9f1dc71 369
OkiAviation 0:2576f9f1dc71 370 void TinyGPSDate::commit()
OkiAviation 0:2576f9f1dc71 371 {
OkiAviation 0:2576f9f1dc71 372 date = newDate;
OkiAviation 0:2576f9f1dc71 373 lastCommitTime = millis();
OkiAviation 0:2576f9f1dc71 374 valid = updated = true;
OkiAviation 0:2576f9f1dc71 375 }
OkiAviation 0:2576f9f1dc71 376
OkiAviation 0:2576f9f1dc71 377 void TinyGPSTime::commit()
OkiAviation 0:2576f9f1dc71 378 {
OkiAviation 0:2576f9f1dc71 379 time = newTime;
OkiAviation 0:2576f9f1dc71 380 lastCommitTime = millis();
OkiAviation 0:2576f9f1dc71 381 valid = updated = true;
OkiAviation 0:2576f9f1dc71 382 }
OkiAviation 0:2576f9f1dc71 383
OkiAviation 0:2576f9f1dc71 384 void TinyGPSTime::setTime(const char *term)
OkiAviation 0:2576f9f1dc71 385 {
OkiAviation 0:2576f9f1dc71 386 newTime = (uint32_t)TinyGPSPlus::parseDecimal(term);
OkiAviation 0:2576f9f1dc71 387 }
OkiAviation 0:2576f9f1dc71 388
OkiAviation 0:2576f9f1dc71 389 void TinyGPSDate::setDate(const char *term)
OkiAviation 0:2576f9f1dc71 390 {
OkiAviation 0:2576f9f1dc71 391 newDate = atol(term);
OkiAviation 0:2576f9f1dc71 392 }
OkiAviation 0:2576f9f1dc71 393
OkiAviation 0:2576f9f1dc71 394 uint16_t TinyGPSDate::year()
OkiAviation 0:2576f9f1dc71 395 {
OkiAviation 0:2576f9f1dc71 396 updated = false;
OkiAviation 0:2576f9f1dc71 397 uint16_t year = date % 100;
OkiAviation 0:2576f9f1dc71 398 return year + 2000;
OkiAviation 0:2576f9f1dc71 399 }
OkiAviation 0:2576f9f1dc71 400
OkiAviation 0:2576f9f1dc71 401 uint8_t TinyGPSDate::month()
OkiAviation 0:2576f9f1dc71 402 {
OkiAviation 0:2576f9f1dc71 403 updated = false;
OkiAviation 0:2576f9f1dc71 404 return (date / 100) % 100;
OkiAviation 0:2576f9f1dc71 405 }
OkiAviation 0:2576f9f1dc71 406
OkiAviation 0:2576f9f1dc71 407 uint8_t TinyGPSDate::day()
OkiAviation 0:2576f9f1dc71 408 {
OkiAviation 0:2576f9f1dc71 409 updated = false;
OkiAviation 0:2576f9f1dc71 410 return date / 10000;
OkiAviation 0:2576f9f1dc71 411 }
OkiAviation 0:2576f9f1dc71 412
OkiAviation 0:2576f9f1dc71 413 uint8_t TinyGPSTime::hour()
OkiAviation 0:2576f9f1dc71 414 {
OkiAviation 0:2576f9f1dc71 415 updated = false;
OkiAviation 0:2576f9f1dc71 416 return time / 1000000;
OkiAviation 0:2576f9f1dc71 417 }
OkiAviation 0:2576f9f1dc71 418
OkiAviation 0:2576f9f1dc71 419 uint8_t TinyGPSTime::minute()
OkiAviation 0:2576f9f1dc71 420 {
OkiAviation 0:2576f9f1dc71 421 updated = false;
OkiAviation 0:2576f9f1dc71 422 return (time / 10000) % 100;
OkiAviation 0:2576f9f1dc71 423 }
OkiAviation 0:2576f9f1dc71 424
OkiAviation 0:2576f9f1dc71 425 uint8_t TinyGPSTime::second()
OkiAviation 0:2576f9f1dc71 426 {
OkiAviation 0:2576f9f1dc71 427 updated = false;
OkiAviation 0:2576f9f1dc71 428 return (time / 100) % 100;
OkiAviation 0:2576f9f1dc71 429 }
OkiAviation 0:2576f9f1dc71 430
OkiAviation 0:2576f9f1dc71 431 uint8_t TinyGPSTime::centisecond()
OkiAviation 0:2576f9f1dc71 432 {
OkiAviation 0:2576f9f1dc71 433 updated = false;
OkiAviation 0:2576f9f1dc71 434 return time % 100;
OkiAviation 0:2576f9f1dc71 435 }
OkiAviation 0:2576f9f1dc71 436
OkiAviation 0:2576f9f1dc71 437 void TinyGPSDecimal::commit()
OkiAviation 0:2576f9f1dc71 438 {
OkiAviation 0:2576f9f1dc71 439 val = newval;
OkiAviation 0:2576f9f1dc71 440 lastCommitTime = millis();
OkiAviation 0:2576f9f1dc71 441 valid = updated = true;
OkiAviation 0:2576f9f1dc71 442 }
OkiAviation 0:2576f9f1dc71 443
OkiAviation 0:2576f9f1dc71 444 void TinyGPSDecimal::set(const char *term)
OkiAviation 0:2576f9f1dc71 445 {
OkiAviation 0:2576f9f1dc71 446 newval = TinyGPSPlus::parseDecimal(term);
OkiAviation 0:2576f9f1dc71 447 }
OkiAviation 0:2576f9f1dc71 448
OkiAviation 0:2576f9f1dc71 449 void TinyGPSInteger::commit()
OkiAviation 0:2576f9f1dc71 450 {
OkiAviation 0:2576f9f1dc71 451 val = newval;
OkiAviation 0:2576f9f1dc71 452 lastCommitTime = millis();
OkiAviation 0:2576f9f1dc71 453 valid = updated = true;
OkiAviation 0:2576f9f1dc71 454 }
OkiAviation 0:2576f9f1dc71 455
OkiAviation 0:2576f9f1dc71 456 void TinyGPSInteger::set(const char *term)
OkiAviation 0:2576f9f1dc71 457 {
OkiAviation 0:2576f9f1dc71 458 newval = atol(term);
OkiAviation 0:2576f9f1dc71 459 }
OkiAviation 0:2576f9f1dc71 460
OkiAviation 0:2576f9f1dc71 461 TinyGPSCustom::TinyGPSCustom(TinyGPSPlus &gps, const char *_sentenceName, int _termNumber)
OkiAviation 0:2576f9f1dc71 462 {
OkiAviation 0:2576f9f1dc71 463 begin(gps, _sentenceName, _termNumber);
OkiAviation 0:2576f9f1dc71 464 }
OkiAviation 0:2576f9f1dc71 465
OkiAviation 0:2576f9f1dc71 466 void TinyGPSCustom::begin(TinyGPSPlus &gps, const char *_sentenceName, int _termNumber)
OkiAviation 0:2576f9f1dc71 467 {
OkiAviation 0:2576f9f1dc71 468 lastCommitTime = 0;
OkiAviation 0:2576f9f1dc71 469 updated = valid = false;
OkiAviation 0:2576f9f1dc71 470 sentenceName = _sentenceName;
OkiAviation 0:2576f9f1dc71 471 termNumber = _termNumber;
OkiAviation 0:2576f9f1dc71 472 memset(stagingBuffer, '\0', sizeof(stagingBuffer));
OkiAviation 0:2576f9f1dc71 473 memset(buffer, '\0', sizeof(buffer));
OkiAviation 0:2576f9f1dc71 474
OkiAviation 0:2576f9f1dc71 475 // Insert this item into the GPS tree
OkiAviation 0:2576f9f1dc71 476 gps.insertCustom(this, _sentenceName, _termNumber);
OkiAviation 0:2576f9f1dc71 477 }
OkiAviation 0:2576f9f1dc71 478
OkiAviation 0:2576f9f1dc71 479 void TinyGPSCustom::commit()
OkiAviation 0:2576f9f1dc71 480 {
OkiAviation 0:2576f9f1dc71 481 strcpy(this->buffer, this->stagingBuffer);
OkiAviation 0:2576f9f1dc71 482 lastCommitTime = millis();
OkiAviation 0:2576f9f1dc71 483 valid = updated = true;
OkiAviation 0:2576f9f1dc71 484 }
OkiAviation 0:2576f9f1dc71 485
OkiAviation 0:2576f9f1dc71 486 void TinyGPSCustom::set(const char *term)
OkiAviation 0:2576f9f1dc71 487 {
OkiAviation 0:2576f9f1dc71 488 strncpy(this->stagingBuffer, term, sizeof(this->stagingBuffer));
OkiAviation 0:2576f9f1dc71 489 }
OkiAviation 0:2576f9f1dc71 490
OkiAviation 0:2576f9f1dc71 491 void TinyGPSPlus::insertCustom(TinyGPSCustom *pElt, const char *sentenceName, int termNumber)
OkiAviation 0:2576f9f1dc71 492 {
OkiAviation 0:2576f9f1dc71 493 TinyGPSCustom **ppelt;
OkiAviation 0:2576f9f1dc71 494
OkiAviation 0:2576f9f1dc71 495 for (ppelt = &this->customElts; *ppelt != NULL; ppelt = &(*ppelt)->next)
OkiAviation 0:2576f9f1dc71 496 {
OkiAviation 0:2576f9f1dc71 497 int cmp = strcmp(sentenceName, (*ppelt)->sentenceName);
OkiAviation 0:2576f9f1dc71 498 if (cmp < 0 || (cmp == 0 && termNumber < (*ppelt)->termNumber))
OkiAviation 0:2576f9f1dc71 499 break;
OkiAviation 0:2576f9f1dc71 500 }
OkiAviation 0:2576f9f1dc71 501
OkiAviation 0:2576f9f1dc71 502 pElt->next = *ppelt;
OkiAviation 0:2576f9f1dc71 503 *ppelt = pElt;
OkiAviation 0:2576f9f1dc71 504 }