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 uint16_t ao_gps_tick;
32 __xdata struct ao_gps_data ao_gps_data;
33 __xdata struct ao_gps_tracking_data ao_gps_tracking_data;
35 static __xdata uint16_t ao_gps_next_tick;
36 static __xdata struct ao_gps_data ao_gps_next;
37 static __xdata uint8_t ao_gps_date_flags;
38 static __xdata struct ao_gps_tracking_data ao_gps_tracking_next;
40 #define STQ_S 0xa0, 0xa1
41 #define STQ_E 0x0d, 0x0a
42 #define SKYTRAQ_MSG_2(id,a,b) \
43 STQ_S, 0, 3, id, a,b, (id^a^b), STQ_E
44 #define SKYTRAQ_MSG_3(id,a,b,c) \
45 STQ_S, 0, 4, id, a,b,c, (id^a^b^c), STQ_E
46 #define SKYTRAQ_MSG_8(id,a,b,c,d,e,f,g,h) \
47 STQ_S, 0, 9, id, a,b,c,d,e,f,g,h, (id^a^b^c^d^e^f^g^h), STQ_E
48 #define SKYTRAQ_MSG_14(id,a,b,c,d,e,f,g,h,i,j,k,l,m,n) \
49 STQ_S, 0,15, id, a,b,c,d,e,f,g,h,i,j,k,l,m,n, \
50 (id^a^b^c^d^e^f^g^h^i^j^k^l^m^n), STQ_E
52 static const uint8_t ao_gps_config_serial[] = {
53 SKYTRAQ_MSG_3(0x05, 0, 4, 0), /* set serial port */
55 /* 0 = 4800, 1 = 9600, 2 = 19200, 3 = 38400,
56 * 4 = 57600, 5 = 115200 */
59 static const uint8_t ao_gps_config[] = {
60 SKYTRAQ_MSG_8(0x08, 1, 1, 1, 1, 1, 1, 1, 0), /* configure nmea */
68 /* attributes (0 = update to sram, 1 = update flash too) */
70 SKYTRAQ_MSG_2(0x3c, 0x00, 0x00), /* configure navigation mode */
71 /* 0 = car, 1 = pedestrian */
72 /* 0 = update to sram, 1 = update sram + flash */
74 SKYTRAQ_MSG_2(0x0e, 10, 0), /* config nav interval */
76 /* 0 = update to sram, 1 = update sram */
85 ao_gps_char = ao_serial_getchar();
86 ao_gps_cksum ^= ao_gps_char;
90 ao_gps_skip_field(void)
92 while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
99 if (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
103 __xdata static uint8_t ao_gps_num_width;
106 ao_gps_decimal(uint8_t max_width)
109 __xdata uint8_t neg = 0;
112 if (ao_gps_char == '-') {
117 ao_gps_num_width = 0;
118 while (ao_gps_num_width < max_width) {
119 if (ao_gps_char < '0' || '9' < ao_gps_char)
121 v = v * (int16_t) 10 + ao_gps_char - '0';
131 ao_gps_hex(uint8_t max_width)
137 ao_gps_num_width = 0;
138 while (ao_gps_num_width < max_width) {
139 if ('0' <= ao_gps_char && ao_gps_char <= '9')
140 d = ao_gps_char - '0';
141 else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
142 d = ao_gps_char - 'A' + 10;
143 else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
144 d = ao_gps_char - 'a' + 10;
155 ao_gps_parse_pos(uint8_t deg_width) __reentrant
161 d = ao_gps_decimal(deg_width);
162 m = ao_gps_decimal(2);
163 if (ao_gps_char == '.') {
164 f = ao_gps_decimal(4);
165 while (ao_gps_num_width < 4) {
171 if (ao_gps_char != ',')
181 ao_gps_parse_flag(char no_c, char yes_c) __reentrant
185 if (ao_gps_char == yes_c)
187 else if (ao_gps_char == no_c)
200 /* Now read the data into the gps data record
202 * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
206 * 025149.000 time (02:51:49.000 GMT)
207 * 4528.1723,N Latitude 45°28.1723' N
208 * 12244.2480,W Longitude 122°44.2480' W
214 * 4 = Real Time Kinematic
216 * 6 = estimated (dead reckoning)
217 * 7 = Manual input mode
218 * 8 = Simulation mode
219 * 05 Number of satellites (5)
220 * 2.0 Horizontal dilution
221 * 103.5,M Altitude, 103.5M above msl
222 * -19.5,M Height of geoid above WGS84 ellipsoid
223 * ? time in seconds since last DGPS update
224 * 0000 DGPS station ID
228 ao_gps_next_tick = ao_time();
229 ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
230 ao_gps_next.hour = ao_gps_decimal(2);
231 ao_gps_next.minute = ao_gps_decimal(2);
232 ao_gps_next.second = ao_gps_decimal(2);
233 ao_gps_skip_field(); /* skip seconds fraction */
235 ao_gps_next.latitude = ao_gps_parse_pos(2);
236 if (ao_gps_parse_flag('N', 'S'))
237 ao_gps_next.latitude = -ao_gps_next.latitude;
238 ao_gps_next.longitude = ao_gps_parse_pos(3);
239 if (ao_gps_parse_flag('E', 'W'))
240 ao_gps_next.longitude = -ao_gps_next.longitude;
242 i = ao_gps_decimal(0xff);
244 ao_gps_next.flags |= AO_GPS_VALID;
246 i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
247 if (i > AO_GPS_NUM_SAT_MASK)
248 i = AO_GPS_NUM_SAT_MASK;
249 ao_gps_next.flags |= i;
252 ao_gps_next.hdop = ao_gps_decimal(0xff);
253 if (ao_gps_next.hdop <= 50) {
254 ao_gps_next.hdop = (uint8_t) 5 * ao_gps_next.hdop;
255 if (ao_gps_char == '.')
256 ao_gps_next.hdop = (ao_gps_next.hdop +
257 ((uint8_t) ao_gps_decimal(1) >> 1));
259 ao_gps_next.hdop = 255;
262 ao_gps_next.altitude = ao_gps_decimal(0xff);
263 ao_gps_skip_field(); /* skip any fractional portion */
265 /* Skip remaining fields */
266 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
270 if (ao_gps_char == '*') {
271 uint8_t cksum = ao_gps_cksum ^ '*';
272 if (cksum != ao_gps_hex(2))
277 ao_mutex_get(&ao_gps_mutex);
278 ao_gps_tick = ao_gps_next_tick;
279 memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data));
280 ao_mutex_put(&ao_gps_mutex);
281 ao_wakeup(&ao_gps_data);
291 /* Now read the data into the GPS tracking data record
293 * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
295 * Satellites in view data
297 * 3 Total number of GSV messages
298 * 1 Sequence number of current GSV message
299 * 12 Total sats in view (0-12)
307 c = ao_gps_decimal(1); /* total messages */
308 i = ao_gps_decimal(1); /* message sequence */
310 ao_gps_tracking_next.channels = 0;
312 done = (uint8_t) c == i;
314 ao_gps_skip_field(); /* sats in view */
315 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
316 i = ao_gps_tracking_next.channels;
317 c = ao_gps_decimal(2); /* SVID */
318 if (i < AO_MAX_GPS_TRACKING)
319 ao_gps_tracking_next.sats[i].svid = c;
321 ao_gps_skip_field(); /* elevation */
323 ao_gps_skip_field(); /* azimuth */
324 c = ao_gps_decimal(2); /* C/N0 */
325 if (i < AO_MAX_GPS_TRACKING) {
326 if (!(ao_gps_tracking_next.sats[i].c_n_1 = c))
327 ao_gps_tracking_next.sats[i].svid = 0;
328 ao_gps_tracking_next.channels = i + 1;
331 if (ao_gps_char == '*') {
332 uint8_t cksum = ao_gps_cksum ^ '*';
333 if (cksum != ao_gps_hex(2))
339 ao_gps_tracking_next.channels = 0;
341 ao_mutex_get(&ao_gps_mutex);
342 memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next,
343 sizeof(ao_gps_tracking_data));
344 ao_mutex_put(&ao_gps_mutex);
345 ao_wakeup(&ao_gps_tracking_data);
354 /* Parse the RMC record to read out the current date */
356 /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
358 * Recommended Minimum Specific GNSS Data
360 * 111636.932 UTC time 11:16:36.932
361 * A Data Valid (V = receiver warning)
363 * N North/south indicator
364 * 12100.5223 Longitude
365 * E East/west indicator
366 * 000.0 Speed over ground
367 * 000.0 Course over ground
368 * 030407 UTC date (ddmmyy format)
371 * A = autonomous mode
372 * D = differential mode
373 * E = estimated (dead reckoning) mode
374 * M = manual input mode
379 for (i = 0; i < 8; i++) {
383 a = ao_gps_decimal(2);
384 c = ao_gps_decimal(2);
385 i = ao_gps_decimal(2);
386 /* Skip remaining fields */
387 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
391 if (ao_gps_char == '*') {
392 uint8_t cksum = ao_gps_cksum ^ '*';
393 if (cksum != ao_gps_hex(2))
398 ao_gps_next.year = i;
399 ao_gps_next.month = c;
401 ao_gps_date_flags = AO_GPS_DATE_VALID;
405 #define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), (s)+sizeof(s))
408 ao_skytraq_sendbytes(const uint8_t *b, const uint8_t *e)
412 ao_delay(AO_MS_TO_TICKS(500));
413 ao_serial_putchar(*b++);
418 ao_gps_nmea_parse(void)
425 for (a = 0; a < AO_GPS_LEADER; a++) {
427 if (ao_gps_char != ao_gps_header[a])
439 if (ao_gps_char != ',')
442 if (a == (uint8_t) 'G' && b == (uint8_t) 'G' && c == (uint8_t) 'A') {
444 } else if (a == (uint8_t) 'G' && b == (uint8_t) 'S' && c == (uint8_t) 'V') {
446 } else if (a == (uint8_t) 'R' && b == (uint8_t) 'M' && c == (uint8_t) 'C') {
452 ao_gps(void) __reentrant
454 ao_serial_set_speed(AO_SERIAL_SPEED_9600);
456 /* give skytraq time to boot in case of cold start */
457 ao_delay(AO_MS_TO_TICKS(2000));
458 ao_skytraq_sendstruct(ao_gps_config_serial);
459 ao_delay(AO_MS_TO_TICKS(1000));
460 ao_serial_set_speed(AO_SERIAL_SPEED_57600);
462 ao_skytraq_sendstruct(ao_gps_config);
465 /* Locate the begining of the next record */
466 if (ao_serial_getchar() == '$') {
473 __xdata struct ao_task ao_gps_task;
476 gps_dump(void) __reentrant
478 ao_mutex_get(&ao_gps_mutex);
479 printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
480 printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
481 printf ("Lat/Lon: %ld %ld\n", ao_gps_data.latitude, ao_gps_data.longitude);
482 printf ("Alt: %d\n", ao_gps_data.altitude);
483 printf ("Flags: 0x%x\n", ao_gps_data.flags);
484 ao_mutex_put(&ao_gps_mutex);
487 __code struct ao_cmds ao_gps_cmds[] = {
488 { 'g', gps_dump, "g Display current GPS values" },
489 { 0, gps_dump, NULL },
495 ao_add_task(&ao_gps_task, ao_gps, "gps");
496 ao_cmd_register(&ao_gps_cmds[0]);