e793de519ebf553eae49e107c189a046d03aaafd
[fw/altos] / src / ao_gps.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 __xdata uint8_t ao_gps_mutex;
23 __xdata struct ao_gps_data      ao_gps_data;
24
25 #undef AO_GPS_4800
26
27 #ifdef AO_GPS_4800
28 static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n\r\n";
29 #else
30 static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,4800,8,1,0*0F\r\n\r\n";
31 #endif
32
33 const char ao_gps_config[] = {
34         0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */
35         136,                    /* mode control */
36         0, 0,                   /* reserved */
37         4,                      /* degraded mode (disabled) */
38         0, 0,                   /* reserved */
39         0, 0,                   /* user specified altitude */
40         2,                      /* alt hold mode (disabled, require 3d fixes) */
41         0,                      /* alt hold source (use last computed altitude) */
42         0,                      /* reserved */
43         0,                      /* Degraded time out (disabled) */
44         0,                      /* Dead Reckoning time out (disabled) */
45         0,                      /* Track smoothing (disabled) */
46         0x00, 0x8e, 0xb0, 0xb3,
47
48         0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */
49         166,                    /* Set message rate */
50         2,                      /* enable/disable all messages */
51         0,                      /* message id (ignored) */
52         0,                      /* update rate (0 = disable) */
53         0, 0, 0, 0,             /* reserved */
54         0x00, 0xa8, 0xb0, 0xb3,
55
56         0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */
57         143,                    /* static navigation */
58         0,                      /* disable */
59         0x00, 0x8f, 0xb0, 0xb3,
60 };
61
62 #define NAV_TYPE_GPS_FIX_TYPE_MASK                      (7 << 0)
63 #define NAV_TYPE_NO_FIX                                 (0 << 0)
64 #define NAV_TYPE_SV_KF                                  (1 << 0)
65 #define NAV_TYPE_2_SV_KF                                (2 << 0)
66 #define NAV_TYPE_3_SV_KF                                (3 << 0)
67 #define NAV_TYPE_4_SV_KF                                (4 << 0)
68 #define NAV_TYPE_2D_LEAST_SQUARES                       (5 << 0)
69 #define NAV_TYPE_3D_LEAST_SQUARES                       (6 << 0)
70 #define NAV_TYPE_DR                                     (7 << 0)
71 #define NAV_TYPE_TRICKLE_POWER                          (1 << 3)
72 #define NAV_TYPE_ALTITUDE_HOLD_MASK                     (3 << 4)
73 #define NAV_TYPE_ALTITUDE_HOLD_NONE                     (0 << 4)
74 #define NAV_TYPE_ALTITUDE_HOLD_KF                       (1 << 4)
75 #define NAV_TYPE_ALTITUDE_HOLD_USER                     (2 << 4)
76 #define NAV_TYPE_ALTITUDE_HOLD_ALWAYS                   (3 << 4)
77 #define NAV_TYPE_DOP_LIMIT_EXCEEDED                     (1 << 6)
78 #define NAV_TYPE_DGPS_APPLIED                           (1 << 7)
79 #define NAV_TYPE_SENSOR_DR                              (1 << 8)
80 #define NAV_TYPE_OVERDETERMINED                         (1 << 9)
81 #define NAV_TYPE_DR_TIMEOUT_EXCEEDED                    (1 << 10)
82 #define NAV_TYPE_FIX_MI_EDIT                            (1 << 11)
83 #define NAV_TYPE_INVALID_VELOCITY                       (1 << 12)
84 #define NAV_TYPE_ALTITUDE_HOLD_DISABLED                 (1 << 13)
85 #define NAV_TYPE_DR_ERROR_STATUS_MASK                   (3 << 14)
86 #define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY               (0 << 14)
87 #define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS            (1 << 14)
88 #define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR        (2 << 14)
89 #define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST             (3 << 14)
90
91 struct sirf_geodetic_nav_data {
92         uint16_t        nav_type;
93         uint16_t        utc_year;
94         uint8_t         utc_month;
95         uint8_t         utc_day;
96         uint8_t         utc_hour;
97         uint8_t         utc_minute;
98         uint16_t        utc_second;
99         int32_t         lat;
100         int32_t         lon;
101         int32_t         alt_msl;
102         uint16_t        ground_speed;
103         uint16_t        course;
104         int16_t         climb_rate;
105         uint32_t        h_error;
106         uint32_t        v_error;
107         uint8_t         num_sv;
108         uint8_t         hdop;
109 };
110
111 static __xdata struct sirf_geodetic_nav_data    ao_sirf_data;
112
113 static __pdata uint16_t ao_sirf_cksum;
114 static __pdata uint16_t ao_sirf_len;
115
116 #define ao_sirf_byte()  ((uint8_t) ao_serial_getchar())
117
118 static uint8_t data_byte(void)
119 {
120         uint8_t c = ao_sirf_byte();
121         --ao_sirf_len;
122         ao_sirf_cksum += c;
123         return c;
124 }
125
126 static void sirf_u16(uint8_t offset)
127 {
128         uint16_t __xdata *ptr = (uint16_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
129         uint16_t val;
130
131         val = data_byte() << 8;
132         val |= data_byte ();
133         *ptr = val;
134 }
135
136 static void sirf_u8(uint8_t offset)
137 {
138         uint8_t __xdata *ptr = (uint8_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
139         uint8_t val;
140
141         val = data_byte ();
142         *ptr = val;
143 }
144
145 static void sirf_u32(uint8_t offset)
146 {
147         uint32_t __xdata *ptr = (uint32_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
148         uint32_t val;
149
150         val = ((uint32_t) data_byte ()) << 24;
151         val |= ((uint32_t) data_byte ()) << 16;
152         val |= ((uint32_t) data_byte ()) << 8;
153         val |= ((uint32_t) data_byte ());
154         *ptr = val;
155 }
156
157 static void sirf_discard(uint8_t len)
158 {
159         while (len--)
160                 data_byte();
161 }
162
163 #define SIRF_END        0
164 #define SIRF_DISCARD    1
165 #define SIRF_U8         2
166 #define SIRF_U16        3
167 #define SIRF_U32        4
168
169 struct sirf_packet_parse {
170         uint8_t type;
171         uint8_t offset;
172 };
173
174 static const struct sirf_packet_parse geodetic_nav_data_packet[] = {
175         { SIRF_DISCARD, 2 },                                                    /* 1 nav valid */
176         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) },        /* 3 */
177         { SIRF_DISCARD, 6 },                                                    /* 5 week number, time of week */
178         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) },        /* 11 */
179         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) },        /* 13 */
180         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) },          /* 14 */
181         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) },         /* 15 */
182         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) },       /* 16 */
183         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) },      /* 17 */
184         { SIRF_DISCARD, 4 },    /* satellite id list */                         /* 19 */
185         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) },             /* 23 */
186         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) },             /* 27 */
187         { SIRF_DISCARD, 4 },    /* altitude from ellipsoid */                   /* 31 */
188         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) },         /* 35 */
189         { SIRF_DISCARD, 1 },    /* map datum */                                 /* 39 */
190         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) },    /* 40 */
191         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) },          /* 42 */
192         { SIRF_DISCARD, 2 },    /* magnetic variation */                        /* 44 */
193         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) },      /* 46 */
194         { SIRF_DISCARD, 2 },    /* turn rate */                                 /* 48 */
195         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) },         /* 50 */
196         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) },         /* 54 */
197         { SIRF_DISCARD, 30 },   /* time error, h_vel error, clock_bias,
198                                    clock bias error, clock drift,
199                                    clock drift error, distance,
200                                    distance error, heading error */             /* 58 */
201         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) },           /* 88 */
202         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) },             /* 89 */
203         { SIRF_DISCARD, 1 },    /* additional mode info */                      /* 90 */
204         { SIRF_END, 0 },                                                        /* 91 */
205 };
206
207 static void
208 ao_sirf_parse_41(void)
209 {
210         uint8_t i, offset;
211
212         for (i = 0; ; i++) {
213                 offset = geodetic_nav_data_packet[i].offset;
214                 switch (geodetic_nav_data_packet[i].type) {
215                 case SIRF_END:
216                         return;
217                 case SIRF_DISCARD:
218                         sirf_discard(offset);
219                         break;
220                 case SIRF_U8:
221                         sirf_u8(offset);
222                         break;
223                 case SIRF_U16:
224                         sirf_u16(offset);
225                         break;
226                 case SIRF_U32:
227                         sirf_u32(offset);
228                         break;
229                 }
230         }
231 }
232
233 static void
234 ao_gps_setup(void) __reentrant
235 {
236         uint8_t i, k;
237 #ifdef AO_GPS_4800
238         ao_serial_set_speed(AO_SERIAL_SPEED_4800);
239 #endif
240         for (i = 0; i < 64; i++)
241                 ao_serial_putchar(0x00);
242         for (k = 0; k < 3; k++)
243                 for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
244                         ao_serial_putchar(ao_gps_set_nmea[i]);
245 #ifdef AO_GPS_4800
246         ao_serial_set_speed(AO_SERIAL_SPEED_57600);
247 #endif
248         for (i = 0; i < 64; i++)
249                 ao_serial_putchar(0x00);
250 }
251
252 static const char ao_gps_set_message_rate[] = {
253         0xa0, 0xa2, 0x00, 0x08,
254         166,
255         0,
256 };
257
258 void
259 ao_sirf_set_message_rate(uint8_t msg, uint8_t rate)
260 {
261         uint16_t        cksum = 0x00a6;
262         uint8_t         i;
263
264         for (i = 0; i < sizeof (ao_gps_set_message_rate); i++)
265                 ao_serial_putchar(ao_gps_set_message_rate[i]);
266         ao_serial_putchar(msg);
267         ao_serial_putchar(rate);
268         cksum = 0xa6 + msg + rate;
269         for (i = 0; i < 4; i++)
270                 ao_serial_putchar(0);
271         ao_serial_putchar((cksum >> 8) & 0x7f);
272         ao_serial_putchar(cksum & 0xff);
273         ao_serial_putchar(0xb0);
274         ao_serial_putchar(0xb3);
275 }
276
277 static const uint8_t sirf_disable[] = {
278         2,
279         4,
280         9,
281         10,
282         27,
283         50,
284         52,
285 };
286
287 void
288 ao_gps(void) __reentrant
289 {
290         uint8_t i, k;
291         uint16_t cksum;
292
293         ao_gps_setup();
294         for (k = 0; k < 5; k++)
295         {
296                 for (i = 0; i < sizeof (ao_gps_config); i++)
297                         ao_serial_putchar(ao_gps_config[i]);
298                 for (i = 0; i < sizeof (sirf_disable); i++)
299                         ao_sirf_set_message_rate(sirf_disable[i], 0);
300                 ao_sirf_set_message_rate(41, 1);
301         }
302         for (;;) {
303                 /* Locate the begining of the next record */
304                 while (ao_sirf_byte() != (uint8_t) 0xa0)
305                         ;
306                 if (ao_sirf_byte() != (uint8_t) 0xa2)
307                         continue;
308
309                 /* Length */
310                 ao_sirf_len = ao_sirf_byte() << 8;
311                 ao_sirf_len |= ao_sirf_byte();
312                 if (ao_sirf_len > 1023)
313                         continue;
314
315                 ao_sirf_cksum = 0;
316
317                 /* message ID */
318                 i = data_byte ();                                                       /* 0 */
319
320                 switch (i) {
321                 case 41:
322                         if (ao_sirf_len < 90)
323                                 break;
324                         ao_sirf_parse_41();
325                         break;
326                 }
327                 if (ao_sirf_len != 0)
328                         continue;
329
330                 /* verify checksum and end sequence */
331                 ao_sirf_cksum &= 0x7fff;
332                 cksum = ao_sirf_byte() << 8;
333                 cksum |= ao_sirf_byte();
334                 if (ao_sirf_cksum != cksum)
335                         continue;
336                 if (ao_sirf_byte() != (uint8_t) 0xb0)
337                         continue;
338                 if (ao_sirf_byte() != (uint8_t) 0xb3)
339                         continue;
340
341                 switch (i) {
342                 case 41:
343                         ao_mutex_get(&ao_gps_mutex);
344                         ao_gps_data.hour = ao_sirf_data.utc_hour;
345                         ao_gps_data.minute = ao_sirf_data.utc_minute;
346                         ao_gps_data.second = ao_sirf_data.utc_second / 1000;
347                         ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING;
348                         if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF)
349                                 ao_gps_data.flags |= AO_GPS_VALID;
350                         ao_gps_data.latitude = ao_sirf_data.lat;
351                         ao_gps_data.longitude = ao_sirf_data.lon;
352                         ao_gps_data.altitude = ao_sirf_data.alt_msl / 100;
353                         ao_gps_data.ground_speed = ao_sirf_data.ground_speed;
354                         ao_gps_data.course = ao_sirf_data.course / 200;
355                         ao_gps_data.hdop = ao_sirf_data.hdop;
356                         ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
357                         if (ao_sirf_data.h_error > 6553500)
358                                 ao_gps_data.h_error = 65535;
359                         else
360                                 ao_gps_data.h_error = ao_sirf_data.h_error / 100;
361                         if (ao_sirf_data.v_error > 6553500)
362                                 ao_gps_data.v_error = 65535;
363                         else
364                                 ao_gps_data.v_error = ao_sirf_data.v_error / 100;
365                         ao_mutex_put(&ao_gps_mutex);
366                         ao_wakeup(&ao_gps_data);
367                         break;
368                 }
369         }
370 }
371
372 __xdata struct ao_task ao_gps_task;
373
374 static void
375 gps_dump(void) __reentrant
376 {
377         ao_mutex_get(&ao_gps_mutex);
378         ao_gps_print(&ao_gps_data);
379         ao_mutex_put(&ao_gps_mutex);
380 }
381
382 __code struct ao_cmds ao_gps_cmds[] = {
383         { 'g', gps_dump,        "g                                  Display current GPS values" },
384         { 0, gps_dump, NULL },
385 };
386
387 void
388 ao_gps_init(void)
389 {
390         ao_add_task(&ao_gps_task, ao_gps, "gps");
391         ao_cmd_register(&ao_gps_cmds[0]);
392 }