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