2 * Copyright © 2009 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; version 2 of the License.
8 * This program is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * General Public License for more details.
13 * You should have received a copy of the GNU General Public License along
14 * with this program; if not, write to the Free Software Foundation, Inc.,
15 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
22 #define AO_GPS_LEADER 2
24 static const char ao_gps_header[] = "GP";
26 __xdata uint8_t ao_gps_mutex;
27 static __xdata char ao_gps_char;
28 static __xdata uint8_t ao_gps_cksum;
29 static __xdata uint8_t ao_gps_error;
31 __xdata struct ao_gps_data ao_gps_data;
32 __xdata struct ao_gps_tracking_data ao_gps_tracking_data;
34 static __xdata struct ao_gps_data ao_gps_next;
35 static __xdata uint8_t ao_gps_date_flags;
36 static __xdata struct ao_gps_tracking_data ao_gps_tracking_next;
38 static const char ao_gps_config[] = {
39 0xa0, 0xa1, 0x00, 0x09, /* length 9 bytes */
40 0x08, /* configure nmea */
48 0, /* attributes (0 = update to sram, 1 = update flash too) */
51 0xa0, 0xa1, 0x00, 0x03, /* length: 3 bytes */
52 0x3c, /* configure navigation mode */
53 0x00, /* 0 = car, 1 = pedestrian */
54 0x00, /* 0 = update to sram, 1 = update sram + flash */
64 ao_gps_char = ao_serial_getchar();
65 ao_gps_cksum ^= ao_gps_char;
71 while (ao_gps_char >= '0')
76 ao_gps_skip_field(void)
78 while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
85 if (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
89 __xdata static uint8_t ao_gps_num_width;
92 ao_gps_decimal(uint8_t max_width)
95 __xdata uint8_t neg = 0;
98 if (ao_gps_char == '-') {
103 ao_gps_num_width = 0;
104 while (ao_gps_num_width < max_width) {
105 if (ao_gps_char < '0' || '9' < ao_gps_char)
107 v = v * (int16_t) 10 + ao_gps_char - '0';
117 ao_gps_hex(uint8_t max_width)
123 ao_gps_num_width = 0;
124 while (ao_gps_num_width < max_width) {
125 if ('0' <= ao_gps_char && ao_gps_char <= '9')
126 d = ao_gps_char - '0';
127 else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
128 d = ao_gps_char - 'A' + 10;
129 else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
130 d = ao_gps_char - 'a' + 10;
141 ao_gps_parse_pos(uint8_t deg_width) __reentrant
147 d = ao_gps_decimal(deg_width);
148 m = ao_gps_decimal(2);
149 if (ao_gps_char == '.') {
150 f = ao_gps_decimal(4);
151 while (ao_gps_num_width < 4) {
157 if (ao_gps_char != ',')
167 ao_gps_parse_flag(char no_c, char yes_c) __reentrant
171 if (ao_gps_char == yes_c)
173 else if (ao_gps_char == no_c)
183 ao_gps(void) __reentrant
188 ao_serial_set_speed(AO_SERIAL_SPEED_9600);
189 for (i = 0; i < sizeof (ao_gps_config); i++)
190 ao_serial_putchar(ao_gps_config[i]);
192 /* Locate the begining of the next record */
194 c = ao_serial_getchar();
202 /* Skip anything other than GP */
203 for (i = 0; i < AO_GPS_LEADER; i++) {
205 if (ao_gps_char != ao_gps_header[i])
208 if (i != AO_GPS_LEADER)
211 /* pull the record identifier characters off the link */
219 if (ao_gps_char != ',')
222 if (a == (uint8_t) 'G' && c == (uint8_t) 'G' && i == (uint8_t) 'A') {
223 /* Now read the data into the gps data record
225 * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
229 * 025149.000 time (02:51:49.000 GMT)
230 * 4528.1723,N Latitude 45°28.1723' N
231 * 12244.2480,W Longitude 122°44.2480' W
237 * 4 = Real Time Kinematic
239 * 6 = estimated (dead reckoning)
240 * 7 = Manual input mode
241 * 8 = Simulation mode
242 * 05 Number of satellites (5)
243 * 2.0 Horizontal dilution
244 * 103.5,M Altitude, 103.5M above msl
245 * -19.5,M Height of geoid above WGS84 ellipsoid
246 * ? time in seconds since last DGPS update
247 * 0000 DGPS station ID
251 ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
252 ao_gps_next.hour = ao_gps_decimal(2);
253 ao_gps_next.minute = ao_gps_decimal(2);
254 ao_gps_next.second = ao_gps_decimal(2);
255 ao_gps_skip_field(); /* skip seconds fraction */
257 ao_gps_next.latitude = ao_gps_parse_pos(2);
258 if (ao_gps_parse_flag('N', 'S'))
259 ao_gps_next.latitude = -ao_gps_next.latitude;
260 ao_gps_next.longitude = ao_gps_parse_pos(3);
261 if (ao_gps_parse_flag('E', 'W'))
262 ao_gps_next.longitude = -ao_gps_next.longitude;
264 i = ao_gps_decimal(0xff);
266 ao_gps_next.flags |= AO_GPS_VALID;
268 i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
269 if (i > AO_GPS_NUM_SAT_MASK)
270 i = AO_GPS_NUM_SAT_MASK;
271 ao_gps_next.flags |= i;
274 ao_gps_next.hdop = ao_gps_decimal(0xff);
275 if (ao_gps_next.hdop <= 50) {
276 ao_gps_next.hdop = (uint8_t) 5 * ao_gps_next.hdop;
277 if (ao_gps_char == '.')
278 ao_gps_next.hdop = (ao_gps_next.hdop +
279 ((uint8_t) ao_gps_decimal(1) >> 1));
281 ao_gps_next.hdop = 255;
284 ao_gps_next.altitude = ao_gps_decimal(0xff);
285 ao_gps_skip_field(); /* skip any fractional portion */
287 /* Skip remaining fields */
288 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
292 if (ao_gps_char == '*') {
293 uint8_t cksum = ao_gps_cksum ^ '*';
294 if (cksum != ao_gps_hex(2))
299 ao_mutex_get(&ao_gps_mutex);
300 memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data));
301 ao_mutex_put(&ao_gps_mutex);
302 ao_wakeup(&ao_gps_data);
304 } else if (a == (uint8_t) 'G' && c == (uint8_t) 'S' && i == (uint8_t) 'V') {
306 /* Now read the data into the GPS tracking data record
308 * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
310 * Satellites in view data
312 * 3 Total number of GSV messages
313 * 1 Sequence number of current GSV message
314 * 12 Total sats in view (0-12)
322 c = ao_gps_decimal(1); /* total messages */
323 i = ao_gps_decimal(1); /* message sequence */
325 ao_gps_tracking_next.channels = 0;
327 done = (uint8_t) c == i;
329 ao_gps_skip_field(); /* sats in view */
330 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
331 i = ao_gps_tracking_next.channels;
332 ao_gps_tracking_next.sats[i].svid = ao_gps_decimal(2); /* SVID */
334 ao_gps_skip_field(); /* elevation */
336 ao_gps_skip_field(); /* azimuth */
337 if (!(ao_gps_tracking_next.sats[i].c_n_1 = ao_gps_decimal(2))) /* C/N0 */
338 ao_gps_tracking_next.sats[i].svid = 0;
339 ao_gps_tracking_next.channels = i + 1;
341 if (ao_gps_char == '*') {
342 uint8_t cksum = ao_gps_cksum ^ '*';
343 if (cksum != ao_gps_hex(2))
349 ao_gps_tracking_next.channels = 0;
351 ao_mutex_get(&ao_gps_mutex);
352 memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next,
353 sizeof(ao_gps_tracking_data));
354 ao_mutex_put(&ao_gps_mutex);
355 ao_wakeup(&ao_gps_tracking_data);
357 } else if (a == (uint8_t) 'R' && c == (uint8_t) 'M' && i == (uint8_t) 'C') {
358 /* Parse the RMC record to read out the current date */
360 /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
362 * Recommended Minimum Specific GNSS Data
364 * 111636.932 UTC time 11:16:36.932
365 * A Data Valid (V = receiver warning)
367 * N North/south indicator
368 * 12100.5223 Longitude
369 * E East/west indicator
370 * 000.0 Speed over ground
371 * 000.0 Course over ground
372 * 030407 UTC date (ddmmyy format)
375 * A = autonomous mode
376 * D = differential mode
377 * E = estimated (dead reckoning) mode
378 * M = manual input mode
383 for (i = 0; i < 8; i++) {
387 a = ao_gps_decimal(2);
388 c = ao_gps_decimal(2);
389 i = ao_gps_decimal(2);
390 /* Skip remaining fields */
391 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
395 if (ao_gps_char == '*') {
396 uint8_t cksum = ao_gps_cksum ^ '*';
397 if (cksum != ao_gps_hex(2))
402 ao_gps_next.year = i;
403 ao_gps_next.month = c;
405 ao_gps_date_flags = AO_GPS_DATE_VALID;
411 __xdata struct ao_task ao_gps_task;
414 gps_dump(void) __reentrant
416 ao_mutex_get(&ao_gps_mutex);
417 printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
418 printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
419 printf ("Lat/Lon: %ld %ld\n", ao_gps_data.latitude, ao_gps_data.longitude);
420 printf ("Alt: %d\n", ao_gps_data.altitude);
421 ao_mutex_put(&ao_gps_mutex);
424 __code struct ao_cmds ao_gps_cmds[] = {
425 { 'g', gps_dump, "g Display current GPS values" },
426 { 0, gps_dump, NULL },
432 ao_add_task(&ao_gps_task, ao_gps, "gps");
433 ao_cmd_register(&ao_gps_cmds[0]);