Clément BENOIT / TinyGPS

My version of TinyGPS mostly add dbg

Dependents:   PYRN

Fork of TinyGPS by Michael Shimniok

TinyGPS.cpp@2:34b4815eb82f, 2015-04-14 (annotated)

Committer:
clemounet
Date:
Tue Apr 14 13:24:45 2015 +0000
Revision:
2:34b4815eb82f
Parent:
1:f522b8bdf987 
MyTinyGps version (moslty dbg stuffs)

Who changed what in which revision?

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