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 #ifndef ao_gps_getchar
23 #define ao_gps_getchar ao_serial1_getchar
24 #define ao_gps_fifo ao_serial1_rx_fifo
27 #ifndef ao_gps_putchar
28 #define ao_gps_putchar ao_serial1_putchar
31 #ifndef ao_gps_set_speed
32 #define ao_gps_set_speed ao_serial1_set_speed
35 __xdata uint8_t ao_gps_mutex;
36 static __data char ao_gps_char;
37 static __data uint8_t ao_gps_cksum;
38 static __data uint8_t ao_gps_error;
40 __pdata uint16_t ao_gps_tick;
41 __xdata struct ao_telemetry_location ao_gps_data;
42 __xdata struct ao_telemetry_satellite ao_gps_tracking_data;
44 static __pdata uint16_t ao_gps_next_tick;
45 static __pdata struct ao_telemetry_location ao_gps_next;
46 static __pdata uint8_t ao_gps_date_flags;
47 static __pdata struct ao_telemetry_satellite ao_gps_tracking_next;
49 #define STQ_S 0xa0, 0xa1
50 #define STQ_E 0x0d, 0x0a
51 #define SKYTRAQ_MSG_2(id,a,b) \
52 STQ_S, 0, 3, id, a,b, (id^a^b), STQ_E
53 #define SKYTRAQ_MSG_3(id,a,b,c) \
54 STQ_S, 0, 4, id, a,b,c, (id^a^b^c), STQ_E
55 #define SKYTRAQ_MSG_8(id,a,b,c,d,e,f,g,h) \
56 STQ_S, 0, 9, id, a,b,c,d,e,f,g,h, (id^a^b^c^d^e^f^g^h), STQ_E
57 #define SKYTRAQ_MSG_14(id,a,b,c,d,e,f,g,h,i,j,k,l,m,n) \
58 STQ_S, 0,15, id, a,b,c,d,e,f,g,h,i,j,k,l,m,n, \
59 (id^a^b^c^d^e^f^g^h^i^j^k^l^m^n), STQ_E
61 static __code uint8_t ao_gps_config[] = {
62 SKYTRAQ_MSG_8(0x08, 1, 0, 1, 0, 1, 0, 0, 0), /* configure nmea */
70 /* attributes (0 = update to sram, 1 = update flash too) */
72 SKYTRAQ_MSG_2(0x3c, 0x00, 0x00), /* configure navigation mode */
73 /* 0 = car, 1 = pedestrian */
74 /* 0 = update to sram, 1 = update sram + flash */
90 ao_gps_skip_field(void)
94 if (c == ',' || c == '*' || c == '\n')
101 ao_gps_skip_sep(void)
103 char c = ao_gps_char;
104 if (c == ',' || c == '.' || c == '*')
108 __data static uint8_t ao_gps_num_width;
111 ao_gps_decimal(uint8_t max_width)
117 if (ao_gps_char == '-') {
122 ao_gps_num_width = 0;
123 while (ao_gps_num_width < max_width) {
124 uint8_t c = ao_gps_char;
125 if (c < (uint8_t) '0' || (uint8_t) '9' < c)
127 v = v * 10 + (uint8_t) (c - (uint8_t) '0');
143 ao_gps_num_width = 0;
144 while (ao_gps_num_width < 2) {
145 uint8_t c = ao_gps_char;
147 if ((uint8_t) '0' <= c && c <= (uint8_t) '9')
149 else if ((uint8_t) 'A' <= c && c <= (uint8_t) 'F')
151 else if ((uint8_t) 'a' <= c && c <= (uint8_t) 'f')
155 v = (v << 4) | (c + d);
163 ao_gps_parse_pos(uint8_t deg_width) __reentrant
165 static __pdata uint16_t d;
166 static __pdata uint8_t m;
167 static __pdata uint16_t f;
170 d = ao_gps_decimal(deg_width);
171 m = ao_gps_decimal(2);
174 f = ao_gps_decimal(4);
175 while (ao_gps_num_width < 4) {
184 return d * 10000000l + (m * 10000l + f) * 50 / 3;
188 ao_gps_parse_flag(char no_c, char yes_c)
192 if (ao_gps_char == yes_c)
194 else if (ao_gps_char == no_c)
206 /* Skip remaining fields */
209 if (c == '*' || c == '\n' || c == '\r')
215 uint8_t cksum = ao_gps_cksum ^ '*';
216 if (cksum != ao_gps_hex())
227 /* Now read the data into the gps data record
229 * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
233 * 025149.000 time (02:51:49.000 GMT)
234 * 4528.1723,N Latitude 45°28.1723' N
235 * 12244.2480,W Longitude 122°44.2480' W
241 * 4 = Real Time Kinematic
243 * 6 = estimated (dead reckoning)
244 * 7 = Manual input mode
245 * 8 = Simulation mode
246 * 05 Number of satellites (5)
247 * 2.0 Horizontal dilution
248 * 103.5,M Altitude, 103.5M above msl
249 * -19.5,M Height of geoid above WGS84 ellipsoid
250 * ? time in seconds since last DGPS update
251 * 0000 DGPS station ID
255 ao_gps_next_tick = ao_time();
256 ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
257 ao_gps_next.hour = ao_gps_decimal(2);
258 ao_gps_next.minute = ao_gps_decimal(2);
259 ao_gps_next.second = ao_gps_decimal(2);
260 ao_gps_skip_field(); /* skip seconds fraction */
262 ao_gps_next.latitude = ao_gps_parse_pos(2);
263 if (ao_gps_parse_flag('N', 'S'))
264 ao_gps_next.latitude = -ao_gps_next.latitude;
265 ao_gps_next.longitude = ao_gps_parse_pos(3);
266 if (ao_gps_parse_flag('E', 'W'))
267 ao_gps_next.longitude = -ao_gps_next.longitude;
269 i = ao_gps_decimal(0xff);
271 ao_gps_next.flags |= AO_GPS_VALID;
273 i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
274 if (i > AO_GPS_NUM_SAT_MASK)
275 i = AO_GPS_NUM_SAT_MASK;
276 ao_gps_next.flags |= i;
279 i = ao_gps_decimal(0xff);
282 if (ao_gps_char == '.')
283 i = (i + ((uint8_t) ao_gps_decimal(1) >> 1));
286 ao_gps_next.hdop = i;
289 ao_gps_next.altitude = ao_gps_decimal(0xff);
290 ao_gps_skip_field(); /* skip any fractional portion */
295 ao_mutex_get(&ao_gps_mutex);
296 ao_gps_tick = ao_gps_next_tick;
297 ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data));
298 ao_mutex_put(&ao_gps_mutex);
299 ao_wakeup(&ao_gps_data);
309 /* Now read the data into the GPS tracking data record
311 * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
313 * Satellites in view data
315 * 3 Total number of GSV messages
316 * 1 Sequence number of current GSV message
317 * 12 Total sats in view (0-12)
325 c = ao_gps_decimal(1); /* total messages */
326 i = ao_gps_decimal(1); /* message sequence */
328 ao_gps_tracking_next.channels = 0;
330 done = (uint8_t) c == i;
332 ao_gps_skip_field(); /* sats in view */
333 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
334 i = ao_gps_tracking_next.channels;
335 c = ao_gps_decimal(2); /* SVID */
336 if (i < AO_MAX_GPS_TRACKING)
337 ao_gps_tracking_next.sats[i].svid = c;
339 ao_gps_skip_field(); /* elevation */
341 ao_gps_skip_field(); /* azimuth */
342 c = ao_gps_decimal(2); /* C/N0 */
343 if (i < AO_MAX_GPS_TRACKING) {
344 if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0)
345 ao_gps_tracking_next.channels = i + 1;
352 ao_gps_tracking_next.channels = 0;
354 ao_mutex_get(&ao_gps_mutex);
355 ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data));
356 ao_mutex_put(&ao_gps_mutex);
357 ao_wakeup(&ao_gps_tracking_data);
366 /* Parse the RMC record to read out the current date */
368 /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
370 * Recommended Minimum Specific GNSS Data
372 * 111636.932 UTC time 11:16:36.932
373 * A Data Valid (V = receiver warning)
375 * N North/south indicator
376 * 12100.5223 Longitude
377 * E East/west indicator
378 * 000.0 Speed over ground
379 * 000.0 Course over ground
380 * 030407 UTC date (ddmmyy format)
383 * A = autonomous mode
384 * D = differential mode
385 * E = estimated (dead reckoning) mode
386 * M = manual input mode
391 for (i = 0; i < 8; i++) {
395 a = ao_gps_decimal(2);
396 c = ao_gps_decimal(2);
397 i = ao_gps_decimal(2);
402 ao_gps_next.year = i;
403 ao_gps_next.month = c;
405 ao_gps_date_flags = AO_GPS_DATE_VALID;
409 #define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s))
412 ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l)
417 ao_delay(AO_MS_TO_TICKS(500));
423 ao_gps_nmea_parse(void)
431 if (ao_gps_char != 'G')
434 if (ao_gps_char != 'P')
445 if (ao_gps_char != ',')
448 if (a == (uint8_t) 'G' && b == (uint8_t) 'G' && c == (uint8_t) 'A') {
450 } else if (a == (uint8_t) 'G' && b == (uint8_t) 'S' && c == (uint8_t) 'V') {
452 } else if (a == (uint8_t) 'R' && b == (uint8_t) 'M' && c == (uint8_t) 'C') {
457 static uint8_t ao_gps_updating;
460 ao_gps(void) __reentrant
462 ao_gps_set_speed(AO_SERIAL_SPEED_9600);
464 /* give skytraq time to boot in case of cold start */
465 ao_delay(AO_MS_TO_TICKS(2000));
467 ao_skytraq_sendstruct(ao_gps_config);
470 /* Locate the begining of the next record */
471 if (ao_gps_getchar() == '$') {
475 while (ao_gps_updating) {
476 ao_usb_putchar(ao_gps_getchar());
477 if (ao_fifo_empty(ao_gps_fifo))
484 __xdata struct ao_task ao_gps_task;
487 gps_dump(void) __reentrant
490 ao_mutex_get(&ao_gps_mutex);
491 printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
492 printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
493 printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude);
494 printf ("Alt: %d\n", ao_gps_data.altitude);
495 printf ("Flags: 0x%x\n", ao_gps_data.flags);
496 printf ("Sats: %d", ao_gps_tracking_data.channels);
497 for (i = 0; i < ao_gps_tracking_data.channels; i++)
499 ao_gps_tracking_data.sats[i].svid,
500 ao_gps_tracking_data.sats[i].c_n_1);
502 ao_mutex_put(&ao_gps_mutex);
505 static __code uint8_t ao_gps_115200[] = {
506 SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */
510 ao_gps_set_speed_delay(uint8_t speed) {
511 ao_delay(AO_MS_TO_TICKS(500));
512 ao_gps_set_speed(speed);
513 ao_delay(AO_MS_TO_TICKS(500));
517 gps_update(void) __reentrant
520 ao_task_minimize_latency = 1;
522 ao_timer_set_adc_interval(0);
524 ao_skytraq_sendstruct(ao_gps_115200);
525 ao_gps_set_speed_delay(AO_SERIAL_SPEED_4800);
526 ao_skytraq_sendstruct(ao_gps_115200);
527 ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200);
529 /* It's a binary protocol; abandon attempts to escape */
531 ao_gps_putchar(ao_usb_getchar());
534 __code struct ao_cmds ao_gps_cmds[] = {
535 { gps_dump, "g\0Display GPS" },
536 { gps_update, "U\0Update GPS firmware" },
543 ao_add_task(&ao_gps_task, ao_gps, "gps");
544 ao_cmd_register(&ao_gps_cmds[0]);