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@2:9fb16575b0f8, 2018-11-07 (annotated)

Committer:
WaleedElmughrabi
Date:
Wed Nov 07 03:42:07 2018 +0000
Revision:
2:9fb16575b0f8
Parent:
1:a4d6efebcef8 
UK time

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;
WaleedElmughrabi 2:9fb16575b0f8 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 }