Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Sensors/GPS/TinyGPS/TinyGPS.h@2:fbc6e3cf3ed8, 2013-06-06 (annotated)
- Committer:
- shimniok
- Date:
- Thu Jun 06 13:40:23 2013 +0000
- Revision:
- 2:fbc6e3cf3ed8
- Parent:
- 0:a6a169de725f
Sort-of working version, still some errors with estimation. Added clamp() function.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:a6a169de725f | 1 | /* |
shimniok | 0:a6a169de725f | 2 | TinyGPS - a small GPS library for Arduino providing basic NMEA parsing |
shimniok | 0:a6a169de725f | 3 | Copyright (C) 2008-9 Mikal Hart |
shimniok | 0:a6a169de725f | 4 | All rights reserved. |
shimniok | 0:a6a169de725f | 5 | |
shimniok | 0:a6a169de725f | 6 | This library is free software; you can redistribute it and/or |
shimniok | 0:a6a169de725f | 7 | modify it under the terms of the GNU Lesser General Public |
shimniok | 0:a6a169de725f | 8 | License as published by the Free Software Foundation; either |
shimniok | 0:a6a169de725f | 9 | version 2.1 of the License, or (at your option) any later version. |
shimniok | 0:a6a169de725f | 10 | |
shimniok | 0:a6a169de725f | 11 | This library is distributed in the hope that it will be useful, |
shimniok | 0:a6a169de725f | 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
shimniok | 0:a6a169de725f | 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
shimniok | 0:a6a169de725f | 14 | Lesser General Public License for more details. |
shimniok | 0:a6a169de725f | 15 | |
shimniok | 0:a6a169de725f | 16 | You should have received a copy of the GNU Lesser General Public |
shimniok | 0:a6a169de725f | 17 | License along with this library; if not, write to the Free Software |
shimniok | 0:a6a169de725f | 18 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
shimniok | 0:a6a169de725f | 19 | |
shimniok | 0:a6a169de725f | 20 | Ported to mbed by Michael Shimniok |
shimniok | 0:a6a169de725f | 21 | */ |
shimniok | 0:a6a169de725f | 22 | |
shimniok | 0:a6a169de725f | 23 | #include "mbed.h" |
shimniok | 0:a6a169de725f | 24 | #include "types.h" |
shimniok | 0:a6a169de725f | 25 | |
shimniok | 0:a6a169de725f | 26 | #ifndef TinyGPS_h |
shimniok | 0:a6a169de725f | 27 | #define TinyGPS_h |
shimniok | 0:a6a169de725f | 28 | |
shimniok | 0:a6a169de725f | 29 | #define _GPS_VERSION 9 // software version of this library |
shimniok | 0:a6a169de725f | 30 | #define _GPS_MPH_PER_KNOT 1.15077945 |
shimniok | 0:a6a169de725f | 31 | #define _GPS_MPS_PER_KNOT 0.51444444 |
shimniok | 0:a6a169de725f | 32 | #define _GPS_KMPH_PER_KNOT 1.852 |
shimniok | 0:a6a169de725f | 33 | #define _GPS_MILES_PER_METER 0.00062137112 |
shimniok | 0:a6a169de725f | 34 | #define _GPS_KM_PER_METER 0.001 |
shimniok | 0:a6a169de725f | 35 | //#define _GPS_NO_STATS |
shimniok | 0:a6a169de725f | 36 | |
shimniok | 0:a6a169de725f | 37 | /** TinyGPS - a small GPS library for Arduino providing basic NMEA parsing Copyright (C) 2008-9 Mikal Hart |
shimniok | 0:a6a169de725f | 38 | * All rights reserved. Modified by Michael Shimniok |
shimniok | 0:a6a169de725f | 39 | */ |
shimniok | 0:a6a169de725f | 40 | |
shimniok | 0:a6a169de725f | 41 | class TinyGPS |
shimniok | 0:a6a169de725f | 42 | { |
shimniok | 0:a6a169de725f | 43 | public: |
shimniok | 0:a6a169de725f | 44 | |
shimniok | 0:a6a169de725f | 45 | /** Create a new GPS parsing object for parsing NMEA sentences |
shimniok | 0:a6a169de725f | 46 | */ |
shimniok | 0:a6a169de725f | 47 | TinyGPS(); |
shimniok | 0:a6a169de725f | 48 | |
shimniok | 0:a6a169de725f | 49 | /** Parse a single character received from GPS |
shimniok | 0:a6a169de725f | 50 | * |
shimniok | 0:a6a169de725f | 51 | * @param c is the character received from the GPS |
shimniok | 0:a6a169de725f | 52 | * @returns true if processing ok |
shimniok | 0:a6a169de725f | 53 | */ |
shimniok | 0:a6a169de725f | 54 | bool encode(char c); |
shimniok | 0:a6a169de725f | 55 | |
shimniok | 0:a6a169de725f | 56 | /** Shorthand operator for encode() |
shimniok | 0:a6a169de725f | 57 | */ |
shimniok | 0:a6a169de725f | 58 | TinyGPS &operator << (char c) {encode(c); return *this;} |
shimniok | 0:a6a169de725f | 59 | |
shimniok | 0:a6a169de725f | 60 | /** Return lat/long in hundred thousandths of a degree and age of fix in milliseconds |
shimniok | 0:a6a169de725f | 61 | * @returns latitude is the latitude of the most recent fix that was parsed |
shimniok | 0:a6a169de725f | 62 | * @returns longitude is the longitude of the most recent fix that was parsed |
shimniok | 0:a6a169de725f | 63 | * @returns fix_age is the age of the fix (if available from the NMEA sentences being parsed) |
shimniok | 0:a6a169de725f | 64 | */ |
shimniok | 0:a6a169de725f | 65 | inline void get_position(long *latitude, long *longitude, unsigned long *fix_age = 0) |
shimniok | 0:a6a169de725f | 66 | { |
shimniok | 0:a6a169de725f | 67 | if (latitude) *latitude = _latitude; |
shimniok | 0:a6a169de725f | 68 | if (longitude) *longitude = _longitude; |
shimniok | 0:a6a169de725f | 69 | if (fix_age) *fix_age = _last_position_fix == GPS_INVALID_FIX_TIME ? |
shimniok | 0:a6a169de725f | 70 | GPS_INVALID_AGE : millis() - _last_position_fix; |
shimniok | 0:a6a169de725f | 71 | } |
shimniok | 0:a6a169de725f | 72 | |
shimniok | 0:a6a169de725f | 73 | /** Return the date and time from the parsed NMEA sentences |
shimniok | 0:a6a169de725f | 74 | * |
shimniok | 0:a6a169de725f | 75 | * @returns date as an integer value |
shimniok | 0:a6a169de725f | 76 | * @returns time as an integer value |
shimniok | 0:a6a169de725f | 77 | * @returns fix_age in milliseconds if available |
shimniok | 0:a6a169de725f | 78 | */ |
shimniok | 0:a6a169de725f | 79 | inline void get_datetime(unsigned long *date, unsigned long *time, unsigned long *fix_age = 0) |
shimniok | 0:a6a169de725f | 80 | { |
shimniok | 0:a6a169de725f | 81 | if (date) *date = _date; |
shimniok | 0:a6a169de725f | 82 | if (time) *time = _time; |
shimniok | 0:a6a169de725f | 83 | if (fix_age) *fix_age = _last_time_fix == GPS_INVALID_FIX_TIME ? |
shimniok | 0:a6a169de725f | 84 | GPS_INVALID_AGE : millis() - _last_time_fix; |
shimniok | 0:a6a169de725f | 85 | } |
shimniok | 0:a6a169de725f | 86 | |
shimniok | 0:a6a169de725f | 87 | /** signed altitude in centimeters (from GPGGA sentence) |
shimniok | 0:a6a169de725f | 88 | * @returns altitude in centimeters, integer |
shimniok | 0:a6a169de725f | 89 | */ |
shimniok | 0:a6a169de725f | 90 | inline long altitude() { return _altitude; } |
shimniok | 0:a6a169de725f | 91 | |
shimniok | 0:a6a169de725f | 92 | /** course in last full GPRMC sentence in 100th of a degree |
shimniok | 0:a6a169de725f | 93 | * @returns course as an integer, 100ths of a degree |
shimniok | 0:a6a169de725f | 94 | */ |
shimniok | 0:a6a169de725f | 95 | inline unsigned long course() { return _course; } |
shimniok | 0:a6a169de725f | 96 | |
shimniok | 0:a6a169de725f | 97 | /** speed in last full GPRMC sentence in 100ths of a knot |
shimniok | 0:a6a169de725f | 98 | * @returns speed in 100ths of a knot |
shimniok | 0:a6a169de725f | 99 | */ |
shimniok | 0:a6a169de725f | 100 | unsigned long speed() { return _speed; } |
shimniok | 0:a6a169de725f | 101 | |
shimniok | 0:a6a169de725f | 102 | /* horizontal dilution of position in last full GPGGA sentence in 100ths |
shimniok | 0:a6a169de725f | 103 | * @returns hdop in 100ths |
shimniok | 0:a6a169de725f | 104 | */ |
shimniok | 0:a6a169de725f | 105 | unsigned long hdop() { return _hdop; } |
shimniok | 0:a6a169de725f | 106 | |
shimniok | 0:a6a169de725f | 107 | /** number of satellites tracked in last full GPGGA sentence |
shimniok | 0:a6a169de725f | 108 | * @returns number of satellites tracked |
shimniok | 0:a6a169de725f | 109 | */ |
shimniok | 0:a6a169de725f | 110 | unsigned long sat_count() { return _sat_count; } |
shimniok | 0:a6a169de725f | 111 | |
shimniok | 0:a6a169de725f | 112 | #ifndef _GPS_NO_STATS |
shimniok | 0:a6a169de725f | 113 | void stats(unsigned long *chars, unsigned short *good_sentences, unsigned short *failed_cs); |
shimniok | 0:a6a169de725f | 114 | #endif |
shimniok | 0:a6a169de725f | 115 | |
shimniok | 0:a6a169de725f | 116 | /** returns position as double precision |
shimniok | 0:a6a169de725f | 117 | * |
shimniok | 0:a6a169de725f | 118 | * @returns latitude as double precision |
shimniok | 0:a6a169de725f | 119 | * @returns longitude as double precision |
shimniok | 0:a6a169de725f | 120 | * @returns fix age in milliseconds if available |
shimniok | 0:a6a169de725f | 121 | */ |
shimniok | 0:a6a169de725f | 122 | inline void f_get_position(double *latitude, double *longitude, unsigned long *fix_age = 0) |
shimniok | 0:a6a169de725f | 123 | { |
shimniok | 0:a6a169de725f | 124 | long lat, lon; |
shimniok | 0:a6a169de725f | 125 | get_position(&lat, &lon, fix_age); |
shimniok | 0:a6a169de725f | 126 | // mes 04/27/12 increased fractional precision to 7 digits, was 5 |
shimniok | 0:a6a169de725f | 127 | *latitude = lat / 10000000.0; |
shimniok | 0:a6a169de725f | 128 | *longitude = lon / 10000000.0; |
shimniok | 0:a6a169de725f | 129 | } |
shimniok | 0:a6a169de725f | 130 | |
shimniok | 0:a6a169de725f | 131 | /** Convert date and time of last parsed sentence to integers |
shimniok | 0:a6a169de725f | 132 | * |
shimniok | 0:a6a169de725f | 133 | * @returns year |
shimniok | 0:a6a169de725f | 134 | * @returns month |
shimniok | 0:a6a169de725f | 135 | * @returns day of month |
shimniok | 0:a6a169de725f | 136 | * @returns hour |
shimniok | 0:a6a169de725f | 137 | * @returns minute |
shimniok | 0:a6a169de725f | 138 | * @returns second |
shimniok | 0:a6a169de725f | 139 | * @returns hundreths |
shimniok | 0:a6a169de725f | 140 | * @returns fix_age in milliseconds if available |
shimniok | 0:a6a169de725f | 141 | */ |
shimniok | 0:a6a169de725f | 142 | inline void crack_datetime(int *year, byte *month, byte *day, |
shimniok | 0:a6a169de725f | 143 | byte *hour, byte *minute, byte *second, byte *hundredths = 0, unsigned long *fix_age = 0) |
shimniok | 0:a6a169de725f | 144 | { |
shimniok | 0:a6a169de725f | 145 | unsigned long date, time; |
shimniok | 0:a6a169de725f | 146 | get_datetime(&date, &time, fix_age); |
shimniok | 0:a6a169de725f | 147 | if (year) |
shimniok | 0:a6a169de725f | 148 | { |
shimniok | 0:a6a169de725f | 149 | *year = date % 100; |
shimniok | 0:a6a169de725f | 150 | *year += *year > 80 ? 1900 : 2000; |
shimniok | 0:a6a169de725f | 151 | } |
shimniok | 0:a6a169de725f | 152 | if (month) *month = (date / 100) % 100; |
shimniok | 0:a6a169de725f | 153 | if (day) *day = date / 10000; |
shimniok | 0:a6a169de725f | 154 | if (hour) *hour = time / 1000000; |
shimniok | 0:a6a169de725f | 155 | if (minute) *minute = (time / 10000) % 100; |
shimniok | 0:a6a169de725f | 156 | if (second) *second = (time / 100) % 100; |
shimniok | 0:a6a169de725f | 157 | if (hundredths) *hundredths = time % 100; |
shimniok | 0:a6a169de725f | 158 | } |
shimniok | 0:a6a169de725f | 159 | |
shimniok | 0:a6a169de725f | 160 | /** returns altitude as a float |
shimniok | 0:a6a169de725f | 161 | */ |
shimniok | 0:a6a169de725f | 162 | inline double f_altitude() { return altitude() / 100.0; } |
shimniok | 0:a6a169de725f | 163 | |
shimniok | 0:a6a169de725f | 164 | /** returns course as a float |
shimniok | 0:a6a169de725f | 165 | */ |
shimniok | 0:a6a169de725f | 166 | inline double f_course() { return course() / 100.0; } |
shimniok | 0:a6a169de725f | 167 | |
shimniok | 0:a6a169de725f | 168 | /** returns speed in knots as a float |
shimniok | 0:a6a169de725f | 169 | */ |
shimniok | 0:a6a169de725f | 170 | inline double f_speed_knots() { return speed() / 100.0; } |
shimniok | 0:a6a169de725f | 171 | |
shimniok | 0:a6a169de725f | 172 | /** returns speed in mph as a float |
shimniok | 0:a6a169de725f | 173 | */ |
shimniok | 0:a6a169de725f | 174 | inline double f_speed_mph() { return _GPS_MPH_PER_KNOT * f_speed_knots(); } |
shimniok | 0:a6a169de725f | 175 | |
shimniok | 0:a6a169de725f | 176 | /** returns speed in meters per second as a float |
shimniok | 0:a6a169de725f | 177 | */ |
shimniok | 0:a6a169de725f | 178 | inline double f_speed_mps() { return _GPS_MPS_PER_KNOT * f_speed_knots(); } |
shimniok | 0:a6a169de725f | 179 | |
shimniok | 0:a6a169de725f | 180 | /** returns speed in km per hour as a float |
shimniok | 0:a6a169de725f | 181 | */ |
shimniok | 0:a6a169de725f | 182 | inline double f_speed_kmph() { return _GPS_KMPH_PER_KNOT * f_speed_knots(); } |
shimniok | 0:a6a169de725f | 183 | |
shimniok | 0:a6a169de725f | 184 | /** returns hdop as a float |
shimniok | 0:a6a169de725f | 185 | */ |
shimniok | 0:a6a169de725f | 186 | inline double f_hdop() { return hdop() / 100.0; } |
shimniok | 0:a6a169de725f | 187 | |
shimniok | 0:a6a169de725f | 188 | /** @returns library version |
shimniok | 0:a6a169de725f | 189 | */ |
shimniok | 0:a6a169de725f | 190 | static int library_version() { return _GPS_VERSION; } |
shimniok | 0:a6a169de725f | 191 | |
shimniok | 0:a6a169de725f | 192 | /** determine if all sentences parsed |
shimniok | 0:a6a169de725f | 193 | * |
shimniok | 0:a6a169de725f | 194 | */ |
shimniok | 0:a6a169de725f | 195 | inline bool ready() { return (_rmc_ready && _gga_ready); } |
shimniok | 0:a6a169de725f | 196 | |
shimniok | 0:a6a169de725f | 197 | /** determine if GSV sentence parsed since last reset_ready() |
shimniok | 0:a6a169de725f | 198 | */ |
shimniok | 0:a6a169de725f | 199 | inline bool gsv_ready() { return _gsv_ready; } |
shimniok | 2:fbc6e3cf3ed8 | 200 | |
shimniok | 2:fbc6e3cf3ed8 | 201 | inline bool gga_ready() { return _gga_ready; } |
shimniok | 0:a6a169de725f | 202 | |
shimniok | 2:fbc6e3cf3ed8 | 203 | inline bool rmc_ready() { return _rmc_ready; } |
shimniok | 2:fbc6e3cf3ed8 | 204 | |
shimniok | 0:a6a169de725f | 205 | /** Reset the ready flags for all the parsed sentences |
shimniok | 0:a6a169de725f | 206 | */ |
shimniok | 0:a6a169de725f | 207 | inline void reset_ready() { _gsv_ready = _rmc_ready = _gga_ready = false; } |
shimniok | 0:a6a169de725f | 208 | |
shimniok | 0:a6a169de725f | 209 | enum {GPS_INVALID_AGE = 0xFFFFFFFF, GPS_INVALID_ANGLE = 999999999, GPS_INVALID_ALTITUDE = 999999999, GPS_INVALID_DATE = 0, |
shimniok | 0:a6a169de725f | 210 | GPS_INVALID_TIME = 0xFFFFFFFF, GPS_INVALID_SPEED = 999999999, GPS_INVALID_FIX_TIME = 0xFFFFFFFF}; |
shimniok | 0:a6a169de725f | 211 | |
shimniok | 0:a6a169de725f | 212 | private: |
shimniok | 0:a6a169de725f | 213 | enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_GPGSV, _GPS_SENTENCE_OTHER}; |
shimniok | 0:a6a169de725f | 214 | |
shimniok | 0:a6a169de725f | 215 | // properties |
shimniok | 0:a6a169de725f | 216 | unsigned long _time, _new_time; |
shimniok | 0:a6a169de725f | 217 | unsigned long _date, _new_date; |
shimniok | 0:a6a169de725f | 218 | long _latitude, _new_latitude; |
shimniok | 0:a6a169de725f | 219 | long _longitude, _new_longitude; |
shimniok | 0:a6a169de725f | 220 | long _altitude, _new_altitude; |
shimniok | 0:a6a169de725f | 221 | unsigned long _speed, _new_speed; |
shimniok | 0:a6a169de725f | 222 | unsigned long _course, _new_course; |
shimniok | 0:a6a169de725f | 223 | unsigned long _hdop, _new_hdop; |
shimniok | 0:a6a169de725f | 224 | unsigned int _sat_count, _new_sat_count; |
shimniok | 0:a6a169de725f | 225 | unsigned long _last_time_fix, _new_time_fix; |
shimniok | 0:a6a169de725f | 226 | unsigned long _last_position_fix, _new_position_fix; |
shimniok | 0:a6a169de725f | 227 | |
shimniok | 0:a6a169de725f | 228 | // parsing state variables |
shimniok | 0:a6a169de725f | 229 | byte _parity; |
shimniok | 0:a6a169de725f | 230 | bool _is_checksum_term; |
shimniok | 0:a6a169de725f | 231 | char _term[15]; |
shimniok | 0:a6a169de725f | 232 | byte _sentence_type; |
shimniok | 0:a6a169de725f | 233 | byte _term_number; |
shimniok | 0:a6a169de725f | 234 | byte _term_offset; |
shimniok | 0:a6a169de725f | 235 | bool _gps_data_good; |
shimniok | 0:a6a169de725f | 236 | bool _rmc_ready; |
shimniok | 0:a6a169de725f | 237 | bool _gga_ready; |
shimniok | 0:a6a169de725f | 238 | bool _gsv_ready; |
shimniok | 0:a6a169de725f | 239 | |
shimniok | 0:a6a169de725f | 240 | #ifndef _GPS_NO_STATS |
shimniok | 0:a6a169de725f | 241 | // statistics |
shimniok | 0:a6a169de725f | 242 | unsigned long _encoded_characters; |
shimniok | 0:a6a169de725f | 243 | unsigned short _good_sentences; |
shimniok | 0:a6a169de725f | 244 | unsigned short _failed_checksum; |
shimniok | 0:a6a169de725f | 245 | unsigned short _passed_checksum; |
shimniok | 0:a6a169de725f | 246 | #endif |
shimniok | 0:a6a169de725f | 247 | |
shimniok | 0:a6a169de725f | 248 | // internal utilities |
shimniok | 0:a6a169de725f | 249 | int from_hex(char a); |
shimniok | 0:a6a169de725f | 250 | int parse_int(); |
shimniok | 0:a6a169de725f | 251 | unsigned long parse_decimal(); |
shimniok | 0:a6a169de725f | 252 | unsigned long parse_degrees(); |
shimniok | 0:a6a169de725f | 253 | bool term_complete(); |
shimniok | 0:a6a169de725f | 254 | bool gpsisdigit(char c) { return c >= '0' && c <= '9'; } |
shimniok | 0:a6a169de725f | 255 | long gpsatol(const char *str); |
shimniok | 0:a6a169de725f | 256 | int gpsstrcmp(const char *str1, const char *str2); |
shimniok | 0:a6a169de725f | 257 | }; |
shimniok | 0:a6a169de725f | 258 | |
shimniok | 0:a6a169de725f | 259 | // Arduino 0012 workaround |
shimniok | 0:a6a169de725f | 260 | #undef int |
shimniok | 0:a6a169de725f | 261 | #undef char |
shimniok | 0:a6a169de725f | 262 | #undef long |
shimniok | 0:a6a169de725f | 263 | #undef byte |
shimniok | 0:a6a169de725f | 264 | #undef double |
shimniok | 0:a6a169de725f | 265 | #undef abs |
shimniok | 0:a6a169de725f | 266 | #undef round |
shimniok | 0:a6a169de725f | 267 | |
shimniok | 0:a6a169de725f | 268 | #endif |