![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Diff: Sensors/GPS/TinyGPS/TinyGPS.cpp
- Revision:
- 0:826c6171fc1b
diff -r 000000000000 -r 826c6171fc1b Sensors/GPS/TinyGPS/TinyGPS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/GPS/TinyGPS/TinyGPS.cpp Wed Jun 20 14:57:48 2012 +0000 @@ -0,0 +1,341 @@ +/* + TinyGPS - a small GPS library for Arduino providing basic NMEA parsing + Copyright (C) 2008-9 Mikal Hart + All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Ported to mbed by Michael Shimniok http://www.bot-thoughts.com/ +*/ + +#include "TinyGPS.h" + +#define _GPRMC_TERM "GPRMC" +#define _GPGGA_TERM "GPGGA" +#define _GPGSV_TERM "GPGSV" + +TinyGPS::TinyGPS() +: _time(GPS_INVALID_TIME) +, _date(GPS_INVALID_DATE) +, _latitude(GPS_INVALID_ANGLE) +, _longitude(GPS_INVALID_ANGLE) +, _altitude(GPS_INVALID_ALTITUDE) +, _speed(GPS_INVALID_SPEED) +, _course(GPS_INVALID_ANGLE) +, _hdop(0) +, _sat_count(0) +, _last_time_fix(GPS_INVALID_FIX_TIME) +, _last_position_fix(GPS_INVALID_FIX_TIME) +, _parity(0) +, _is_checksum_term(false) +, _sentence_type(_GPS_SENTENCE_OTHER) +, _term_number(0) +, _term_offset(0) +, _gps_data_good(false) +, _rmc_ready(false) +, _gga_ready(false) +, _gsv_ready(false) +#ifndef _GPS_NO_STATS +, _encoded_characters(0) +, _good_sentences(0) +, _failed_checksum(0) +#endif +{ + _term[0] = '\0'; +} + +// +// public methods +// + +bool TinyGPS::encode(char c) +{ + bool valid_sentence = false; + + ++_encoded_characters; + switch(c) + { + case ',': // term terminators + _parity ^= c; + case '\r': + case '\n': + case '*': + if (_term_offset < sizeof(_term)) + { + _term[_term_offset] = 0; + valid_sentence = term_complete(); + } + ++_term_number; + _term_offset = 0; + _is_checksum_term = c == '*'; + return valid_sentence; + + case '$': // sentence begin + _term_number = _term_offset = 0; + _parity = 0; + _sentence_type = _GPS_SENTENCE_OTHER; + _is_checksum_term = false; + _gps_data_good = false; + return valid_sentence; + } + + // ordinary characters + if (_term_offset < sizeof(_term) - 1) + _term[_term_offset++] = c; + if (!_is_checksum_term) + _parity ^= c; + + return valid_sentence; +} + +#ifndef _GPS_NO_STATS +void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs) +{ + if (chars) *chars = _encoded_characters; + if (sentences) *sentences = _good_sentences; + if (failed_cs) *failed_cs = _failed_checksum; +} +#endif + +// +// internal utilities +// +int TinyGPS::from_hex(char a) +{ + if (a >= 'A' && a <= 'F') + return a - 'A' + 10; + else if (a >= 'a' && a <= 'f') + return a - 'a' + 10; + else + return a - '0'; +} + +int TinyGPS::parse_int() +{ + char *p = _term; + bool isneg = *p == '-'; + if (isneg) ++p; + while (*p == '0') ++p; + int ret = gpsatol(p); + return isneg ? -ret : ret; +} + +unsigned long TinyGPS::parse_decimal() +{ + char *p = _term; + bool isneg = *p == '-'; + if (isneg) ++p; + unsigned long ret = 100UL * gpsatol(p); + while (gpsisdigit(*p)) ++p; + if (*p == '.') + { + if (gpsisdigit(p[1])) + { + ret += 10 * (p[1] - '0'); + if (gpsisdigit(p[2])) + ret += p[2] - '0'; + } + } + return isneg ? -ret : ret; // TODO: can't return - when we're returning an unsigned! +} + +// mes 04/27/12 increased fractional precision to 7 digits, was 5 +unsigned long TinyGPS::parse_degrees() +{ + char *p; + unsigned long left = gpsatol(_term); + unsigned long tenk_minutes = (left % 100UL) * 1000000UL; + for (p=_term; gpsisdigit(*p); ++p); + if (*p == '.') + { + unsigned long mult = 100000; + while (gpsisdigit(*++p)) + { + tenk_minutes += mult * (*p - '0'); + mult /= 10; + } + } + return (left / 100) * 10000000 + tenk_minutes / 6; +} + +// Processes a just-completed term +// Returns true if new sentence has just passed checksum test and is validated +bool TinyGPS::term_complete() +{ + if (_is_checksum_term) { + + byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]); + if (checksum == _parity) { + + if (_gps_data_good) { + +#ifndef _GPS_NO_STATS + ++_good_sentences; +#endif + _last_time_fix = _new_time_fix; + _last_position_fix = _new_position_fix; + + switch(_sentence_type) { + case _GPS_SENTENCE_GPRMC: + _time = _new_time; + _date = _new_date; + _latitude = _new_latitude; + _longitude = _new_longitude; + _speed = _new_speed; + _course = _new_course; + _rmc_ready = true; + break; + case _GPS_SENTENCE_GPGGA: + _altitude = _new_altitude; + _time = _new_time; + _latitude = _new_latitude; + _longitude = _new_longitude; + _gga_ready = true; + _hdop = _new_hdop; + _sat_count = _new_sat_count; + case _GPS_SENTENCE_GPGSV: + _gsv_ready = true; + break; + } + + return true; + } + } + +#ifndef _GPS_NO_STATS + else + ++_failed_checksum; +#endif + return false; + } + + // the first term determines the sentence type + if (_term_number == 0) { + if (!gpsstrcmp(_term, _GPRMC_TERM)) + _sentence_type = _GPS_SENTENCE_GPRMC; + else if (!gpsstrcmp(_term, _GPGGA_TERM)) + _sentence_type = _GPS_SENTENCE_GPGGA; + else if (!gpsstrcmp(_term, _GPGSV_TERM)) + _sentence_type = _GPS_SENTENCE_GPGSV; + else + _sentence_type = _GPS_SENTENCE_OTHER; + return false; + } + + if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) + /* + if (_sentence_type == _GPS_SENTENCE_GPGSV) { + // $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF> + // TODO: need to maintain a list of 12 structs with sat info and update it each time + switch (_term_number) { + case 0 : // number of messages + break; + case 1 : // sequence number + break; + case 2 : // satellites in view + break; + case 3 : // sat ID + case 8 : + case 12 : + case 16 : + break; + case 4 : // elevation + case 9 : + case 13 : + case 17 : + break; + case 5 : // azimuth + case 10 : + case 14 : + case 18 : + break; + case 6 : // SNR + case 11 : + case 15 : + case 19 : + break; + } + } else {*/ + switch (((_sentence_type == _GPS_SENTENCE_GPGGA) ? 200 : 100) + _term_number) { + case 101: // Time in both sentences + case 201: + _new_time = parse_decimal(); + _new_time_fix = millis(); + break; + case 102: // GPRMC validity + _gps_data_good = _term[0] == 'A'; + break; + case 103: // Latitude + case 202: + _new_latitude = parse_degrees(); + _new_position_fix = millis(); + break; + case 104: // N/S + case 203: + if (_term[0] == 'S') + _new_latitude = -_new_latitude; + break; + case 105: // Longitude + case 204: + _new_longitude = parse_degrees(); + break; + case 106: // E/W + case 205: + if (_term[0] == 'W') + _new_longitude = -_new_longitude; + break; + case 107: // Speed (GPRMC) + _new_speed = parse_decimal(); + break; + case 108: // Course (GPRMC) + _new_course = parse_decimal(); + break; + case 109: // Date (GPRMC) + _new_date = gpsatol(_term); + break; + case 206: // Fix data (GPGGA) + _gps_data_good = _term[0] > '0'; + break; + case 207: // Number of satelites tracked (GPGGA) + _new_sat_count = parse_int(); + break; + case 208: // Horizontal Dilution of Position (GPGGA) + _new_hdop = parse_decimal(); + break; + case 209: // Altitude (GPGGA) + _new_altitude = parse_decimal(); + break; + default : + break; + } /* switch */ + //} + + return false; +} + +long TinyGPS::gpsatol(const char *str) +{ + long ret = 0; + while (gpsisdigit(*str)) + ret = 10 * ret + *str++ - '0'; + return ret; +} + +int TinyGPS::gpsstrcmp(const char *str1, const char *str2) +{ + while (*str1 && *str1 == *str2) + ++str1, ++str2; + return *str1; +}