Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Sensors/GPS/TinyGPS/TinyGPS.cpp@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

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