]> git.gag.com Git - fw/altos/blob - src/ao_gps_skytraq.c
update changelogs for Debian build
[fw/altos] / src / ao_gps_skytraq.c
1 /*
2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
3  *
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.
7  *
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.
12  *
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.
16  */
17
18 #ifndef AO_GPS_TEST
19 #include "ao.h"
20 #endif
21
22 #define AO_GPS_LEADER           2
23
24 static const char ao_gps_header[] = "GP";
25
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;
30
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;
34
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;
39
40 static const char ao_gps_config[] = {
41         0xa0, 0xa1, 0x00, 0x09,         /* length 9 bytes */
42         0x08,                           /* configure nmea */
43         1,                              /* gga interval */
44         1,                              /* gsa interval */
45         1,                              /* gsv interval */
46         1,                              /* gll interval */
47         1,                              /* rmc interval */
48         1,                              /* vtg interval */
49         1,                              /* zda interval */
50         0,                              /* attributes (0 = update to sram, 1 = update flash too) */
51         0x09, 0x0d, 0x0a,
52
53         0xa0, 0xa1, 0x00, 0x03,         /* length: 3 bytes */
54         0x3c,                           /* configure navigation mode */
55         0x00,                           /* 0 = car, 1 = pedestrian */
56         0x00,                           /* 0 = update to sram, 1 = update sram + flash */
57         0x3c, 0x0d, 0x0a,
58 };
59
60 static void
61 ao_gps_lexchar(void)
62 {
63         if (ao_gps_error)
64                 ao_gps_char = '\n';
65         else
66                 ao_gps_char = ao_serial_getchar();
67         ao_gps_cksum ^= ao_gps_char;
68 }
69
70 void
71 ao_gps_skip(void)
72 {
73         while (ao_gps_char >= '0')
74                 ao_gps_lexchar();
75 }
76
77 void
78 ao_gps_skip_field(void)
79 {
80         while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
81                 ao_gps_lexchar();
82 }
83
84 void
85 ao_gps_skip_sep(void)
86 {
87         if (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
88                 ao_gps_lexchar();
89 }
90
91 __xdata static uint8_t ao_gps_num_width;
92
93 static int16_t
94 ao_gps_decimal(uint8_t max_width)
95 {
96         int16_t v;
97         __xdata uint8_t neg = 0;
98
99         ao_gps_skip_sep();
100         if (ao_gps_char == '-') {
101                 neg = 1;
102                 ao_gps_lexchar();
103         }
104         v = 0;
105         ao_gps_num_width = 0;
106         while (ao_gps_num_width < max_width) {
107                 if (ao_gps_char < '0' || '9' < ao_gps_char)
108                         break;
109                 v = v * (int16_t) 10 + ao_gps_char - '0';
110                 ao_gps_num_width++;
111                 ao_gps_lexchar();
112         }
113         if (neg)
114                 v = -v;
115         return v;
116 }
117
118 static uint8_t
119 ao_gps_hex(uint8_t max_width)
120 {
121         uint8_t v, d;
122
123         ao_gps_skip_sep();
124         v = 0;
125         ao_gps_num_width = 0;
126         while (ao_gps_num_width < max_width) {
127                 if ('0' <= ao_gps_char && ao_gps_char <= '9')
128                         d = ao_gps_char - '0';
129                 else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
130                         d = ao_gps_char - 'A' + 10;
131                 else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
132                         d = ao_gps_char - 'a' + 10;
133                 else
134                         break;
135                 v = (v << 4) | d;
136                 ao_gps_num_width++;
137                 ao_gps_lexchar();
138         }
139         return v;
140 }
141
142 static int32_t
143 ao_gps_parse_pos(uint8_t deg_width) __reentrant
144 {
145         int32_t d;
146         int32_t m;
147         int32_t f;
148
149         d = ao_gps_decimal(deg_width);
150         m = ao_gps_decimal(2);
151         if (ao_gps_char == '.') {
152                 f = ao_gps_decimal(4);
153                 while (ao_gps_num_width < 4) {
154                         f *= 10;
155                         ao_gps_num_width++;
156                 }
157         } else {
158                 f = 0;
159                 if (ao_gps_char != ',')
160                         ao_gps_error = 1;
161         }
162         d = d * 10000000l;
163         m = m * 10000l + f;
164         d = d + m * 50 / 3;
165         return d;
166 }
167
168 static uint8_t
169 ao_gps_parse_flag(char no_c, char yes_c) __reentrant
170 {
171         uint8_t ret = 0;
172         ao_gps_skip_sep();
173         if (ao_gps_char == yes_c)
174                 ret = 1;
175         else if (ao_gps_char == no_c)
176                 ret = 0;
177         else
178                 ao_gps_error = 1;
179         ao_gps_lexchar();
180         return ret;
181 }
182
183
184 void
185 ao_gps(void) __reentrant
186 {
187         char    a, c;
188         uint8_t i;
189
190         ao_serial_set_speed(AO_SERIAL_SPEED_9600);
191         for (i = 0; i < sizeof (ao_gps_config); i++)
192                 ao_serial_putchar(ao_gps_config[i]);
193         for (;;) {
194                 /* Locate the begining of the next record */
195                 for (;;) {
196                         c = ao_serial_getchar();
197                         if (c == '$')
198                                 break;
199                 }
200
201                 ao_gps_cksum = 0;
202                 ao_gps_error = 0;
203
204                 /* Skip anything other than GP */
205                 for (i = 0; i < AO_GPS_LEADER; i++) {
206                         ao_gps_lexchar();
207                         if (ao_gps_char != ao_gps_header[i])
208                                 break;
209                 }
210                 if (i != AO_GPS_LEADER)
211                         continue;
212
213                 /* pull the record identifier characters off the link */
214                 ao_gps_lexchar();
215                 a = ao_gps_char;
216                 ao_gps_lexchar();
217                 c = ao_gps_char;
218                 ao_gps_lexchar();
219                 i = ao_gps_char;
220                 ao_gps_lexchar();
221                 if (ao_gps_char != ',')
222                         continue;
223
224                 if (a == (uint8_t) 'G' && c == (uint8_t) 'G' && i == (uint8_t) 'A') {
225                         /* Now read the data into the gps data record
226                          *
227                          * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
228                          *
229                          * Essential fix data
230                          *
231                          *         025149.000   time (02:51:49.000 GMT)
232                          *         4528.1723,N  Latitude 45°28.1723' N
233                          *         12244.2480,W Longitude 122°44.2480' W
234                          *         1            Fix quality:
235                          *                                 0 = invalid
236                          *                                 1 = GPS fix (SPS)
237                          *                                 2 = DGPS fix
238                          *                                 3 = PPS fix
239                          *                                 4 = Real Time Kinematic
240                          *                                 5 = Float RTK
241                          *                                 6 = estimated (dead reckoning)
242                          *                                 7 = Manual input mode
243                          *                                 8 = Simulation mode
244                          *         05           Number of satellites (5)
245                          *         2.0          Horizontal dilution
246                          *         103.5,M              Altitude, 103.5M above msl
247                          *         -19.5,M              Height of geoid above WGS84 ellipsoid
248                          *         ?            time in seconds since last DGPS update
249                          *         0000         DGPS station ID
250                          *         *66          checksum
251                          */
252
253                         ao_gps_next_tick = ao_time();
254                         ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
255                         ao_gps_next.hour = ao_gps_decimal(2);
256                         ao_gps_next.minute = ao_gps_decimal(2);
257                         ao_gps_next.second = ao_gps_decimal(2);
258                         ao_gps_skip_field();    /* skip seconds fraction */
259
260                         ao_gps_next.latitude = ao_gps_parse_pos(2);
261                         if (ao_gps_parse_flag('N', 'S'))
262                                 ao_gps_next.latitude = -ao_gps_next.latitude;
263                         ao_gps_next.longitude = ao_gps_parse_pos(3);
264                         if (ao_gps_parse_flag('E', 'W'))
265                                 ao_gps_next.longitude = -ao_gps_next.longitude;
266
267                         i = ao_gps_decimal(0xff);
268                         if (i == 1)
269                                 ao_gps_next.flags |= AO_GPS_VALID;
270
271                         i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
272                         if (i > AO_GPS_NUM_SAT_MASK)
273                                 i = AO_GPS_NUM_SAT_MASK;
274                         ao_gps_next.flags |= i;
275
276                         ao_gps_lexchar();
277                         ao_gps_next.hdop = ao_gps_decimal(0xff);
278                         if (ao_gps_next.hdop <= 50) {
279                                 ao_gps_next.hdop = (uint8_t) 5 * ao_gps_next.hdop;
280                                 if (ao_gps_char == '.')
281                                         ao_gps_next.hdop = (ao_gps_next.hdop +
282                                                             ((uint8_t) ao_gps_decimal(1) >> 1));
283                         } else
284                                 ao_gps_next.hdop = 255;
285                         ao_gps_skip_field();
286
287                         ao_gps_next.altitude = ao_gps_decimal(0xff);
288                         ao_gps_skip_field();    /* skip any fractional portion */
289
290                         /* Skip remaining fields */
291                         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
292                                 ao_gps_lexchar();
293                                 ao_gps_skip_field();
294                         }
295                         if (ao_gps_char == '*') {
296                                 uint8_t cksum = ao_gps_cksum ^ '*';
297                                 if (cksum != ao_gps_hex(2))
298                                         ao_gps_error = 1;
299                         } else
300                                 ao_gps_error = 1;
301                         if (!ao_gps_error) {
302                                 ao_mutex_get(&ao_gps_mutex);
303                                 ao_gps_tick = ao_gps_next_tick;
304                                 memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data));
305                                 ao_mutex_put(&ao_gps_mutex);
306                                 ao_wakeup(&ao_gps_data);
307                         }
308                 } else if (a == (uint8_t) 'G' && c == (uint8_t) 'S' && i == (uint8_t) 'V') {
309                         uint8_t done;
310                         /* Now read the data into the GPS tracking data record
311                          *
312                          * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
313                          *
314                          * Satellites in view data
315                          *
316                          *      3               Total number of GSV messages
317                          *      1               Sequence number of current GSV message
318                          *      12              Total sats in view (0-12)
319                          *      05              SVID
320                          *      54              Elevation
321                          *      069             Azimuth
322                          *      45              C/N0 in dB
323                          *      ...             other SVIDs
324                          *      72              checksum
325                          */
326                         c = ao_gps_decimal(1);  /* total messages */
327                         i = ao_gps_decimal(1);  /* message sequence */
328                         if (i == 1) {
329                                 ao_gps_tracking_next.channels = 0;
330                         }
331                         done = (uint8_t) c == i;
332                         ao_gps_lexchar();
333                         ao_gps_skip_field();    /* sats in view */
334                         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
335                                 i = ao_gps_tracking_next.channels;
336                                 c = ao_gps_decimal(2);  /* SVID */
337                                 if (i < AO_MAX_GPS_TRACKING)
338                                         ao_gps_tracking_next.sats[i].svid = c;
339                                 ao_gps_lexchar();
340                                 ao_gps_skip_field();    /* elevation */
341                                 ao_gps_lexchar();
342                                 ao_gps_skip_field();    /* azimuth */
343                                 c = ao_gps_decimal(2);  /* C/N0 */
344                                 if (i < AO_MAX_GPS_TRACKING) {
345                                         if (!(ao_gps_tracking_next.sats[i].c_n_1 = c))
346                                                 ao_gps_tracking_next.sats[i].svid = 0;
347                                         ao_gps_tracking_next.channels = i + 1;
348                                 }
349                         }
350                         if (ao_gps_char == '*') {
351                                 uint8_t cksum = ao_gps_cksum ^ '*';
352                                 if (cksum != ao_gps_hex(2))
353                                         ao_gps_error = 1;
354                         }
355                         else
356                                 ao_gps_error = 1;
357                         if (ao_gps_error)
358                                 ao_gps_tracking_next.channels = 0;
359                         else if (done) {
360                                 ao_mutex_get(&ao_gps_mutex);
361                                 memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next,
362                                        sizeof(ao_gps_tracking_data));
363                                 ao_mutex_put(&ao_gps_mutex);
364                                 ao_wakeup(&ao_gps_tracking_data);
365                         }
366                 } else if (a == (uint8_t) 'R' && c == (uint8_t) 'M' && i == (uint8_t) 'C') {
367                         /* Parse the RMC record to read out the current date */
368
369                         /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
370                          *
371                          * Recommended Minimum Specific GNSS Data
372                          *
373                          *      111636.932      UTC time 11:16:36.932
374                          *      A               Data Valid (V = receiver warning)
375                          *      2447.0949       Latitude
376                          *      N               North/south indicator
377                          *      12100.5223      Longitude
378                          *      E               East/west indicator
379                          *      000.0           Speed over ground
380                          *      000.0           Course over ground
381                          *      030407          UTC date (ddmmyy format)
382                          *      A               Mode indicator:
383                          *                      N = data not valid
384                          *                      A = autonomous mode
385                          *                      D = differential mode
386                          *                      E = estimated (dead reckoning) mode
387                          *                      M = manual input mode
388                          *                      S = simulator mode
389                          *      61              checksum
390                          */
391                         ao_gps_skip_field();
392                         for (i = 0; i < 8; i++) {
393                                 ao_gps_lexchar();
394                                 ao_gps_skip_field();
395                         }
396                         a = ao_gps_decimal(2);
397                         c = ao_gps_decimal(2);
398                         i = ao_gps_decimal(2);
399                         /* Skip remaining fields */
400                         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
401                                 ao_gps_lexchar();
402                                 ao_gps_skip_field();
403                         }
404                         if (ao_gps_char == '*') {
405                                 uint8_t cksum = ao_gps_cksum ^ '*';
406                                 if (cksum != ao_gps_hex(2))
407                                         ao_gps_error = 1;
408                         } else
409                                 ao_gps_error = 1;
410                         if (!ao_gps_error) {
411                                 ao_gps_next.year = i;
412                                 ao_gps_next.month = c;
413                                 ao_gps_next.day = a;
414                                 ao_gps_date_flags = AO_GPS_DATE_VALID;
415                         }
416                 }
417         }
418 }
419
420 __xdata struct ao_task ao_gps_task;
421
422 static void
423 gps_dump(void) __reentrant
424 {
425         ao_mutex_get(&ao_gps_mutex);
426         printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
427         printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
428         printf ("Lat/Lon: %ld %ld\n", ao_gps_data.latitude, ao_gps_data.longitude);
429         printf ("Alt: %d\n", ao_gps_data.altitude);
430         printf ("Flags: 0x%x\n", ao_gps_data.flags);
431         ao_mutex_put(&ao_gps_mutex);
432 }
433
434 __code struct ao_cmds ao_gps_cmds[] = {
435         { 'g', gps_dump,        "g                                  Display current GPS values" },
436         { 0,   gps_dump, NULL },
437 };
438
439 void
440 ao_gps_init(void)
441 {
442         ao_add_task(&ao_gps_task, ao_gps, "gps");
443         ao_cmd_register(&ao_gps_cmds[0]);
444 }