Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

GPS/TinyGPS.cpp@43:9a285515f33a, 2019-01-03 (annotated)

Committer:
shimniok
Date:
Thu Jan 03 19:07:20 2019 +0000
Revision:
43:9a285515f33a
implemented tinygps serial, parsing, and callback

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 43:9a285515f33a 1 /*
shimniok 43:9a285515f33a 2 TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
shimniok 43:9a285515f33a 3 Copyright (C) 2008-9 Mikal Hart
shimniok 43:9a285515f33a 4 All rights reserved.
shimniok 43:9a285515f33a 5
shimniok 43:9a285515f33a 6 This library is free software; you can redistribute it and/or
shimniok 43:9a285515f33a 7 modify it under the terms of the GNU Lesser General Public
shimniok 43:9a285515f33a 8 License as published by the Free Software Foundation; either
shimniok 43:9a285515f33a 9 version 2.1 of the License, or (at your option) any later version.
shimniok 43:9a285515f33a 10
shimniok 43:9a285515f33a 11 This library is distributed in the hope that it will be useful,
shimniok 43:9a285515f33a 12 but WITHOUT ANY WARRANTY; without even the implied warranty of
shimniok 43:9a285515f33a 13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
shimniok 43:9a285515f33a 14 Lesser General Public License for more details.
shimniok 43:9a285515f33a 15
shimniok 43:9a285515f33a 16 You should have received a copy of the GNU Lesser General Public
shimniok 43:9a285515f33a 17 License along with this library; if not, write to the Free Software
shimniok 43:9a285515f33a 18 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
shimniok 43:9a285515f33a 19
shimniok 43:9a285515f33a 20 Ported to mbed by Michael Shimniok http://www.bot-thoughts.com/
shimniok 43:9a285515f33a 21 */
shimniok 43:9a285515f33a 22
shimniok 43:9a285515f33a 23 #include "TinyGPS.h"
shimniok 43:9a285515f33a 24
shimniok 43:9a285515f33a 25 #define _GPRMC_TERM "GPRMC"
shimniok 43:9a285515f33a 26 #define _GPGGA_TERM "GPGGA"
shimniok 43:9a285515f33a 27 #define _GPGSV_TERM "GPGSV"
shimniok 43:9a285515f33a 28
shimniok 43:9a285515f33a 29 TinyGPS::TinyGPS()
shimniok 43:9a285515f33a 30 : _time(GPS_INVALID_TIME)
shimniok 43:9a285515f33a 31 , _new_time(GPS_INVALID_TIME)
shimniok 43:9a285515f33a 32 , _date(GPS_INVALID_DATE)
shimniok 43:9a285515f33a 33 , _new_date(GPS_INVALID_DATE)
shimniok 43:9a285515f33a 34 , _latitude(GPS_INVALID_ANGLE)
shimniok 43:9a285515f33a 35 , _new_latitude(GPS_INVALID_ANGLE)
shimniok 43:9a285515f33a 36 , _longitude(GPS_INVALID_ANGLE)
shimniok 43:9a285515f33a 37 , _new_longitude(GPS_INVALID_ANGLE)
shimniok 43:9a285515f33a 38 , _altitude(GPS_INVALID_ALTITUDE)
shimniok 43:9a285515f33a 39 , _new_altitude(GPS_INVALID_ALTITUDE)
shimniok 43:9a285515f33a 40 , _speed(GPS_INVALID_SPEED)
shimniok 43:9a285515f33a 41 , _new_speed(0)
shimniok 43:9a285515f33a 42 , _course(GPS_INVALID_ANGLE)
shimniok 43:9a285515f33a 43 , _new_course(GPS_INVALID_ANGLE)
shimniok 43:9a285515f33a 44 , _hdop(0)
shimniok 43:9a285515f33a 45 , _new_hdop(0)
shimniok 43:9a285515f33a 46 , _sat_count(0)
shimniok 43:9a285515f33a 47 , _new_sat_count(0)
shimniok 43:9a285515f33a 48 , _last_time_fix(GPS_INVALID_FIX_TIME)
shimniok 43:9a285515f33a 49 , _new_time_fix(GPS_INVALID_FIX_TIME)
shimniok 43:9a285515f33a 50 , _last_position_fix(GPS_INVALID_FIX_TIME)
shimniok 43:9a285515f33a 51 , _new_position_fix(GPS_INVALID_FIX_TIME)
shimniok 43:9a285515f33a 52 , _parity(0)
shimniok 43:9a285515f33a 53 , _is_checksum_term(false)
shimniok 43:9a285515f33a 54 , _sentence_type(_GPS_SENTENCE_OTHER)
shimniok 43:9a285515f33a 55 , _term_number(0)
shimniok 43:9a285515f33a 56 , _term_offset(0)
shimniok 43:9a285515f33a 57 , _gps_data_good(false)
shimniok 43:9a285515f33a 58 , _rmc_ready(false)
shimniok 43:9a285515f33a 59 , _gga_ready(false)
shimniok 43:9a285515f33a 60 , _gsv_ready(false)
shimniok 43:9a285515f33a 61 #ifndef _GPS_NO_STATS
shimniok 43:9a285515f33a 62 , _encoded_characters(0)
shimniok 43:9a285515f33a 63 , _good_sentences(0)
shimniok 43:9a285515f33a 64 , _failed_checksum(0)
shimniok 43:9a285515f33a 65 , _passed_checksum(0)
shimniok 43:9a285515f33a 66 #endif
shimniok 43:9a285515f33a 67 {
shimniok 43:9a285515f33a 68 _term[0] = '\0';
shimniok 43:9a285515f33a 69 }
shimniok 43:9a285515f33a 70
shimniok 43:9a285515f33a 71 //
shimniok 43:9a285515f33a 72 // public methods
shimniok 43:9a285515f33a 73 //
shimniok 43:9a285515f33a 74
shimniok 43:9a285515f33a 75 int TinyGPS::parse(char c)
shimniok 43:9a285515f33a 76 {
shimniok 43:9a285515f33a 77 int valid_sentence = 0;
shimniok 43:9a285515f33a 78
shimniok 43:9a285515f33a 79 ++_encoded_characters;
shimniok 43:9a285515f33a 80 switch(c) {
shimniok 43:9a285515f33a 81 case ',': // term terminators
shimniok 43:9a285515f33a 82 _parity ^= c;
shimniok 43:9a285515f33a 83 // no break
shimniok 43:9a285515f33a 84 case '\r':
shimniok 43:9a285515f33a 85 case '\n':
shimniok 43:9a285515f33a 86 case '*':
shimniok 43:9a285515f33a 87 if (_term_offset < sizeof(_term)) {
shimniok 43:9a285515f33a 88 _term[_term_offset] = 0;
shimniok 43:9a285515f33a 89 valid_sentence = term_complete();
shimniok 43:9a285515f33a 90 }
shimniok 43:9a285515f33a 91 ++_term_number;
shimniok 43:9a285515f33a 92 _term_offset = 0;
shimniok 43:9a285515f33a 93 _is_checksum_term = c == '*';
shimniok 43:9a285515f33a 94 if (_callback) _callback();
shimniok 43:9a285515f33a 95 return valid_sentence;
shimniok 43:9a285515f33a 96
shimniok 43:9a285515f33a 97 case '$': // sentence begin
shimniok 43:9a285515f33a 98 _term_number = _term_offset = 0;
shimniok 43:9a285515f33a 99 _parity = 0;
shimniok 43:9a285515f33a 100 _sentence_type = _GPS_SENTENCE_OTHER;
shimniok 43:9a285515f33a 101 _is_checksum_term = false;
shimniok 43:9a285515f33a 102 _gps_data_good = false;
shimniok 43:9a285515f33a 103 return valid_sentence;
shimniok 43:9a285515f33a 104 }
shimniok 43:9a285515f33a 105
shimniok 43:9a285515f33a 106 // ordinary characters
shimniok 43:9a285515f33a 107 if (_term_offset < sizeof(_term) - 1)
shimniok 43:9a285515f33a 108 _term[_term_offset++] = c;
shimniok 43:9a285515f33a 109 if (!_is_checksum_term)
shimniok 43:9a285515f33a 110 _parity ^= c;
shimniok 43:9a285515f33a 111
shimniok 43:9a285515f33a 112 return valid_sentence;
shimniok 43:9a285515f33a 113 }
shimniok 43:9a285515f33a 114
shimniok 43:9a285515f33a 115 #ifndef _GPS_NO_STATS
shimniok 43:9a285515f33a 116 void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
shimniok 43:9a285515f33a 117 {
shimniok 43:9a285515f33a 118 if (chars) *chars = _encoded_characters;
shimniok 43:9a285515f33a 119 if (sentences) *sentences = _good_sentences;
shimniok 43:9a285515f33a 120 if (failed_cs) *failed_cs = _failed_checksum;
shimniok 43:9a285515f33a 121 }
shimniok 43:9a285515f33a 122 #endif
shimniok 43:9a285515f33a 123
shimniok 43:9a285515f33a 124 //
shimniok 43:9a285515f33a 125 // internal utilities
shimniok 43:9a285515f33a 126 //
shimniok 43:9a285515f33a 127 int TinyGPS::from_hex(char a)
shimniok 43:9a285515f33a 128 {
shimniok 43:9a285515f33a 129 if (a >= 'A' && a <= 'F')
shimniok 43:9a285515f33a 130 return a - 'A' + 10;
shimniok 43:9a285515f33a 131 else if (a >= 'a' && a <= 'f')
shimniok 43:9a285515f33a 132 return a - 'a' + 10;
shimniok 43:9a285515f33a 133 else
shimniok 43:9a285515f33a 134 return a - '0';
shimniok 43:9a285515f33a 135 }
shimniok 43:9a285515f33a 136
shimniok 43:9a285515f33a 137 int TinyGPS::parse_int()
shimniok 43:9a285515f33a 138 {
shimniok 43:9a285515f33a 139 char *p = _term;
shimniok 43:9a285515f33a 140 bool isneg = *p == '-';
shimniok 43:9a285515f33a 141 if (isneg) ++p;
shimniok 43:9a285515f33a 142 while (*p == '0') ++p;
shimniok 43:9a285515f33a 143 int ret = gpsatol(p);
shimniok 43:9a285515f33a 144 return isneg ? -ret : ret;
shimniok 43:9a285515f33a 145 }
shimniok 43:9a285515f33a 146
shimniok 43:9a285515f33a 147 long TinyGPS::parse_decimal()
shimniok 43:9a285515f33a 148 {
shimniok 43:9a285515f33a 149 char *p = _term;
shimniok 43:9a285515f33a 150 bool isneg = *p == '-';
shimniok 43:9a285515f33a 151 if (isneg) ++p;
shimniok 43:9a285515f33a 152 unsigned long ret = 100UL * gpsatol(p);
shimniok 43:9a285515f33a 153 while (gpsisdigit(*p)) ++p;
shimniok 43:9a285515f33a 154 if (*p == '.')
shimniok 43:9a285515f33a 155 {
shimniok 43:9a285515f33a 156 if (gpsisdigit(p[1]))
shimniok 43:9a285515f33a 157 {
shimniok 43:9a285515f33a 158 ret += 10 * (p[1] - '0');
shimniok 43:9a285515f33a 159 if (gpsisdigit(p[2]))
shimniok 43:9a285515f33a 160 ret += p[2] - '0';
shimniok 43:9a285515f33a 161 }
shimniok 43:9a285515f33a 162 }
shimniok 43:9a285515f33a 163 return isneg ? -ret : ret;
shimniok 43:9a285515f33a 164 }
shimniok 43:9a285515f33a 165
shimniok 43:9a285515f33a 166 // mes 04/27/12 increased fractional precision to 7 digits, was 5
shimniok 43:9a285515f33a 167 unsigned long TinyGPS::parse_degrees()
shimniok 43:9a285515f33a 168 {
shimniok 43:9a285515f33a 169 char *p;
shimniok 43:9a285515f33a 170 unsigned long left = gpsatol(_term);
shimniok 43:9a285515f33a 171 unsigned long tenk_minutes = (left % 100UL) * 1000000UL;
shimniok 43:9a285515f33a 172 for (p=_term; gpsisdigit(*p); ++p);
shimniok 43:9a285515f33a 173 if (*p == '.')
shimniok 43:9a285515f33a 174 {
shimniok 43:9a285515f33a 175 unsigned long mult = 100000;
shimniok 43:9a285515f33a 176 while (gpsisdigit(*++p))
shimniok 43:9a285515f33a 177 {
shimniok 43:9a285515f33a 178 tenk_minutes += mult * (*p - '0');
shimniok 43:9a285515f33a 179 mult /= 10;
shimniok 43:9a285515f33a 180 }
shimniok 43:9a285515f33a 181 }
shimniok 43:9a285515f33a 182 return (left / 100) * 10000000 + tenk_minutes / 6;
shimniok 43:9a285515f33a 183 }
shimniok 43:9a285515f33a 184
shimniok 43:9a285515f33a 185 // Processes a just-completed term
shimniok 43:9a285515f33a 186 // Returns true if new sentence has just passed checksum test and is validated
shimniok 43:9a285515f33a 187 bool TinyGPS::term_complete()
shimniok 43:9a285515f33a 188 {
shimniok 43:9a285515f33a 189 if (_is_checksum_term) {
shimniok 43:9a285515f33a 190
shimniok 43:9a285515f33a 191 byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
shimniok 43:9a285515f33a 192 if (checksum == _parity) {
shimniok 43:9a285515f33a 193
shimniok 43:9a285515f33a 194 if (_gps_data_good) {
shimniok 43:9a285515f33a 195
shimniok 43:9a285515f33a 196 #ifndef _GPS_NO_STATS
shimniok 43:9a285515f33a 197 ++_good_sentences;
shimniok 43:9a285515f33a 198 #endif
shimniok 43:9a285515f33a 199 _last_time_fix = _new_time_fix;
shimniok 43:9a285515f33a 200 _last_position_fix = _new_position_fix;
shimniok 43:9a285515f33a 201
shimniok 43:9a285515f33a 202 switch(_sentence_type) {
shimniok 43:9a285515f33a 203 case _GPS_SENTENCE_GPRMC:
shimniok 43:9a285515f33a 204 _time = _new_time;
shimniok 43:9a285515f33a 205 _date = _new_date;
shimniok 43:9a285515f33a 206 latest.lat = _new_latitude;
shimniok 43:9a285515f33a 207 latest.lon = _new_longitude;
shimniok 43:9a285515f33a 208 latest.speed = _new_speed;
shimniok 43:9a285515f33a 209 latest.course = _new_course;
shimniok 43:9a285515f33a 210 _rmc_ready = true;
shimniok 43:9a285515f33a 211 break;
shimniok 43:9a285515f33a 212 case _GPS_SENTENCE_GPGGA:
shimniok 43:9a285515f33a 213 _altitude = _new_altitude;
shimniok 43:9a285515f33a 214 _time = _new_time;
shimniok 43:9a285515f33a 215 latest.lat = _new_latitude;
shimniok 43:9a285515f33a 216 latest.lon = _new_longitude;
shimniok 43:9a285515f33a 217 latest.hdop = _new_hdop;
shimniok 43:9a285515f33a 218 latest.svcount = _new_sat_count;
shimniok 43:9a285515f33a 219 _gga_ready = true;
shimniok 43:9a285515f33a 220 break;
shimniok 43:9a285515f33a 221 case _GPS_SENTENCE_GPGSV:
shimniok 43:9a285515f33a 222 _gsv_ready = true;
shimniok 43:9a285515f33a 223 break;
shimniok 43:9a285515f33a 224 }
shimniok 43:9a285515f33a 225
shimniok 43:9a285515f33a 226 return true;
shimniok 43:9a285515f33a 227 }
shimniok 43:9a285515f33a 228 }
shimniok 43:9a285515f33a 229
shimniok 43:9a285515f33a 230 #ifndef _GPS_NO_STATS
shimniok 43:9a285515f33a 231 else
shimniok 43:9a285515f33a 232 ++_failed_checksum;
shimniok 43:9a285515f33a 233 #endif
shimniok 43:9a285515f33a 234 return false;
shimniok 43:9a285515f33a 235 }
shimniok 43:9a285515f33a 236
shimniok 43:9a285515f33a 237 // the first term determines the sentence type
shimniok 43:9a285515f33a 238 if (_term_number == 0) {
shimniok 43:9a285515f33a 239 if (!gpsstrcmp(_term, _GPRMC_TERM))
shimniok 43:9a285515f33a 240 _sentence_type = _GPS_SENTENCE_GPRMC;
shimniok 43:9a285515f33a 241 else if (!gpsstrcmp(_term, _GPGGA_TERM))
shimniok 43:9a285515f33a 242 _sentence_type = _GPS_SENTENCE_GPGGA;
shimniok 43:9a285515f33a 243 else if (!gpsstrcmp(_term, _GPGSV_TERM))
shimniok 43:9a285515f33a 244 _sentence_type = _GPS_SENTENCE_GPGSV;
shimniok 43:9a285515f33a 245 else
shimniok 43:9a285515f33a 246 _sentence_type = _GPS_SENTENCE_OTHER;
shimniok 43:9a285515f33a 247 return false;
shimniok 43:9a285515f33a 248 }
shimniok 43:9a285515f33a 249
shimniok 43:9a285515f33a 250 if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
shimniok 43:9a285515f33a 251 /*
shimniok 43:9a285515f33a 252 if (_sentence_type == _GPS_SENTENCE_GPGSV) {
shimniok 43:9a285515f33a 253 // $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
shimniok 43:9a285515f33a 254 // TODO 4 need to maintain a list of 12 structs with sat info and update it each time
shimniok 43:9a285515f33a 255 switch (_term_number) {
shimniok 43:9a285515f33a 256 case 0 : // number of messages
shimniok 43:9a285515f33a 257 break;
shimniok 43:9a285515f33a 258 case 1 : // sequence number
shimniok 43:9a285515f33a 259 break;
shimniok 43:9a285515f33a 260 case 2 : // satellites in view
shimniok 43:9a285515f33a 261 break;
shimniok 43:9a285515f33a 262 case 3 : // sat ID
shimniok 43:9a285515f33a 263 case 8 :
shimniok 43:9a285515f33a 264 case 12 :
shimniok 43:9a285515f33a 265 case 16 :
shimniok 43:9a285515f33a 266 break;
shimniok 43:9a285515f33a 267 case 4 : // elevation
shimniok 43:9a285515f33a 268 case 9 :
shimniok 43:9a285515f33a 269 case 13 :
shimniok 43:9a285515f33a 270 case 17 :
shimniok 43:9a285515f33a 271 break;
shimniok 43:9a285515f33a 272 case 5 : // azimuth
shimniok 43:9a285515f33a 273 case 10 :
shimniok 43:9a285515f33a 274 case 14 :
shimniok 43:9a285515f33a 275 case 18 :
shimniok 43:9a285515f33a 276 break;
shimniok 43:9a285515f33a 277 case 6 : // SNR
shimniok 43:9a285515f33a 278 case 11 :
shimniok 43:9a285515f33a 279 case 15 :
shimniok 43:9a285515f33a 280 case 19 :
shimniok 43:9a285515f33a 281 break;
shimniok 43:9a285515f33a 282 }
shimniok 43:9a285515f33a 283 } else {*/
shimniok 43:9a285515f33a 284 switch (((_sentence_type == _GPS_SENTENCE_GPGGA) ? 200 : 100) + _term_number) {
shimniok 43:9a285515f33a 285 case 101: // Time in both sentences
shimniok 43:9a285515f33a 286 case 201:
shimniok 43:9a285515f33a 287 _new_time = parse_decimal();
shimniok 43:9a285515f33a 288 //_new_time_fix = millis();
shimniok 43:9a285515f33a 289 break;
shimniok 43:9a285515f33a 290 case 102: // GPRMC validity
shimniok 43:9a285515f33a 291 _gps_data_good = _term[0] == 'A';
shimniok 43:9a285515f33a 292 break;
shimniok 43:9a285515f33a 293 case 103: // Latitude
shimniok 43:9a285515f33a 294 case 202:
shimniok 43:9a285515f33a 295 _new_latitude = parse_degrees();
shimniok 43:9a285515f33a 296 //_new_position_fix = millis();
shimniok 43:9a285515f33a 297 break;
shimniok 43:9a285515f33a 298 case 104: // N/S
shimniok 43:9a285515f33a 299 case 203:
shimniok 43:9a285515f33a 300 if (_term[0] == 'S')
shimniok 43:9a285515f33a 301 _new_latitude = -_new_latitude;
shimniok 43:9a285515f33a 302 break;
shimniok 43:9a285515f33a 303 case 105: // Longitude
shimniok 43:9a285515f33a 304 case 204:
shimniok 43:9a285515f33a 305 _new_longitude = parse_degrees();
shimniok 43:9a285515f33a 306 break;
shimniok 43:9a285515f33a 307 case 106: // E/W
shimniok 43:9a285515f33a 308 case 205:
shimniok 43:9a285515f33a 309 if (_term[0] == 'W')
shimniok 43:9a285515f33a 310 _new_longitude = -_new_longitude;
shimniok 43:9a285515f33a 311 break;
shimniok 43:9a285515f33a 312 case 107: // Speed (GPRMC)
shimniok 43:9a285515f33a 313 _new_speed = parse_decimal();
shimniok 43:9a285515f33a 314 break;
shimniok 43:9a285515f33a 315 case 108: // Course (GPRMC)
shimniok 43:9a285515f33a 316 _new_course = parse_decimal();
shimniok 43:9a285515f33a 317 break;
shimniok 43:9a285515f33a 318 case 109: // Date (GPRMC)
shimniok 43:9a285515f33a 319 _new_date = gpsatol(_term);
shimniok 43:9a285515f33a 320 break;
shimniok 43:9a285515f33a 321 case 206: // Fix data (GPGGA)
shimniok 43:9a285515f33a 322 _gps_data_good = _term[0] > '0';
shimniok 43:9a285515f33a 323 break;
shimniok 43:9a285515f33a 324 case 207: // Number of satelites tracked (GPGGA)
shimniok 43:9a285515f33a 325 _new_sat_count = parse_int();
shimniok 43:9a285515f33a 326 break;
shimniok 43:9a285515f33a 327 case 208: // Horizontal Dilution of Position (GPGGA)
shimniok 43:9a285515f33a 328 _new_hdop = parse_decimal();
shimniok 43:9a285515f33a 329 break;
shimniok 43:9a285515f33a 330 case 209: // Altitude (GPGGA)
shimniok 43:9a285515f33a 331 _new_altitude = parse_decimal();
shimniok 43:9a285515f33a 332 break;
shimniok 43:9a285515f33a 333 default :
shimniok 43:9a285515f33a 334 break;
shimniok 43:9a285515f33a 335 } /* switch */
shimniok 43:9a285515f33a 336 //}
shimniok 43:9a285515f33a 337
shimniok 43:9a285515f33a 338 return false;
shimniok 43:9a285515f33a 339 }
shimniok 43:9a285515f33a 340
shimniok 43:9a285515f33a 341 long TinyGPS::gpsatol(const char *str)
shimniok 43:9a285515f33a 342 {
shimniok 43:9a285515f33a 343 long ret = 0;
shimniok 43:9a285515f33a 344 while (gpsisdigit(*str))
shimniok 43:9a285515f33a 345 ret = 10 * ret + *str++ - '0';
shimniok 43:9a285515f33a 346 return ret;
shimniok 43:9a285515f33a 347 }
shimniok 43:9a285515f33a 348
shimniok 43:9a285515f33a 349 int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
shimniok 43:9a285515f33a 350 {
shimniok 43:9a285515f33a 351 while (*str1 && *str1 == *str2)
shimniok 43:9a285515f33a 352 ++str1, ++str2;
shimniok 43:9a285515f33a 353 return *str1;
shimniok 43:9a285515f33a 354 }