Darrien Galid / Mbed 2 deprecated Fused_Boat_Program

Self-navigating boat program with sensors and control system fused

Dependencies:   mbed Servo PID

TinyGPSplus.cpp@0:cf5854b3296f, 2019-05-10 (annotated)

Committer:
omar28744
Date:
Fri May 10 08:45:41 2019 +0000
Revision:
0:cf5854b3296f
localization program

Who changed what in which revision?

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