2 * Copyright © 2013 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 #include "ao_gps_ublox.h"
24 __xdata uint8_t ao_gps_mutex;
25 __pdata uint16_t ao_gps_tick;
26 __xdata struct ao_telemetry_location ao_gps_data;
27 __xdata struct ao_telemetry_satellite ao_gps_tracking_data;
29 static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n";
31 const char ao_gps_config[] = {
35 struct ao_ublox_cksum {
39 static __pdata struct ao_ublox_cksum ao_ublox_cksum;
40 static __pdata uint16_t ao_ublox_len;
42 #ifndef ao_ublox_getchar
43 #define ao_ublox_getchar ao_serial1_getchar
44 #define ao_ublox_putchar ao_serial1_putchar
45 #define ao_ublox_set_speed ao_serial1_set_speed
48 #define ao_ublox_byte() ((uint8_t) ao_ublox_getchar())
50 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
56 static void ao_ublox_init_cksum(void)
58 ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
61 static void ao_ublox_put_u8(uint8_t c)
63 add_cksum(&ao_ublox_cksum, c);
67 static void ao_ublox_put_i8(int8_t c)
69 ao_ublox_put_u8((uint8_t) c);
72 static void ao_ublox_put_u16(uint16_t c)
75 ao_ublox_put_u8(c>>8);
79 static void ao_ublox_put_i16(int16_t c)
81 ao_ublox_put_u16((uint16_t) c);
85 static void ao_ublox_put_u32(uint32_t c)
88 ao_ublox_put_u8(c>>8);
89 ao_ublox_put_u8(c>>16);
90 ao_ublox_put_u8(c>>24);
93 static void ao_ublox_put_i32(int32_t c)
95 ao_ublox_put_u32((uint32_t) c);
98 static uint8_t header_byte(void)
100 uint8_t c = ao_ublox_byte();
101 add_cksum(&ao_ublox_cksum, c);
105 static uint8_t data_byte(void)
108 return header_byte();
111 static char __xdata *ublox_target;
113 static void ublox_u16(uint8_t offset)
115 uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset);
119 val |= data_byte () << 8;
123 static void ublox_u8(uint8_t offset)
125 uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset);
132 static void ublox_u32(uint8_t offset) __reentrant
134 uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset);
137 val = ((uint32_t) data_byte ());
138 val |= ((uint32_t) data_byte ()) << 8;
139 val |= ((uint32_t) data_byte ()) << 16;
140 val |= ((uint32_t) data_byte ()) << 24;
144 static void ublox_discard(uint8_t len)
151 #define UBLOX_DISCARD 1
156 struct ublox_packet_parse {
162 ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant
166 ublox_target = target;
168 offset = parse[i].offset;
169 switch (parse[i].type) {
173 ublox_discard(offset);
189 * NAV-DOP message parsing
192 static struct nav_dop {
198 static const struct ublox_packet_parse nav_dop_packet[] = {
199 { UBLOX_DISCARD, 6 }, /* 0 GPS Millisecond Time of Week, gDOP */
200 { UBLOX_U16, offsetof(struct nav_dop, pdop) }, /* 6 pDOP */
201 { UBLOX_DISCARD, 2 }, /* 8 tDOP */
202 { UBLOX_U16, offsetof(struct nav_dop, vdop) }, /* 10 vDOP */
203 { UBLOX_U16, offsetof(struct nav_dop, hdop) }, /* 12 hDOP */
204 { UBLOX_DISCARD, 4 }, /* 14 nDOP, eDOP */
209 ao_ublox_parse_nav_dop(void)
211 ao_ublox_parse(&nav_dop, nav_dop_packet);
215 * NAV-POSLLH message parsing
217 static struct nav_posllh {
223 static const struct ublox_packet_parse nav_posllh_packet[] = {
224 { UBLOX_DISCARD, 4 }, /* 0 GPS Millisecond Time of Week */
225 { UBLOX_U32, offsetof(struct nav_posllh, lon) }, /* 4 Longitude */
226 { UBLOX_U32, offsetof(struct nav_posllh, lat) }, /* 8 Latitude */
227 { UBLOX_DISCARD, 4 }, /* 12 Height above Ellipsoid */
228 { UBLOX_U32, offsetof(struct nav_posllh, alt_msl) }, /* 16 Height above mean sea level */
229 { UBLOX_DISCARD, 8 }, /* 20 hAcc, vAcc */
230 { UBLOX_END, 0 }, /* 28 */
234 ao_ublox_parse_nav_posllh(void)
236 ao_ublox_parse(&nav_posllh, nav_posllh_packet);
240 * NAV-SOL message parsing
242 static struct nav_sol {
248 static const struct ublox_packet_parse nav_sol_packet[] = {
249 { UBLOX_DISCARD, 10 }, /* 0 iTOW, fTOW, week */
250 { UBLOX_U8, offsetof(struct nav_sol, gps_fix) }, /* 10 gpsFix */
251 { UBLOX_U8, offsetof(struct nav_sol, flags) }, /* 11 flags */
252 { UBLOX_DISCARD, 35 }, /* 12 ecefX, ecefY, ecefZ, pAcc, ecefVX, ecefVY, ecefVZ, sAcc, pDOP, reserved1 */
253 { UBLOX_U8, offsetof(struct nav_sol, nsat) }, /* 47 numSV */
254 { UBLOX_DISCARD, 4 }, /* 48 reserved2 */
258 #define NAV_SOL_FLAGS_GPSFIXOK 0
259 #define NAV_SOL_FLAGS_DIFFSOLN 1
260 #define NAV_SOL_FLAGS_WKNSET 2
261 #define NAV_SOL_FLAGS_TOWSET 3
264 ao_ublox_parse_nav_sol(void)
266 ao_ublox_parse(&nav_sol, nav_sol_packet);
270 * NAV-SVINFO message parsing
273 static struct nav_svinfo {
278 static const struct ublox_packet_parse nav_svinfo_packet[] = {
279 { UBLOX_DISCARD, 4 }, /* 0 iTOW */
280 { UBLOX_U8, offsetof(struct nav_svinfo, num_ch) }, /* 4 numCh */
281 { UBLOX_U8, offsetof(struct nav_svinfo, flags) }, /* 5 globalFlags */
282 { UBLOX_DISCARD, 2 }, /* 6 reserved2 */
286 #define NAV_SVINFO_MAX_SAT 16
288 static struct nav_svinfo_sat {
294 } nav_svinfo_sat[NAV_SVINFO_MAX_SAT];
296 static uint8_t nav_svinfo_nsat;
298 static const struct ublox_packet_parse nav_svinfo_sat_packet[] = {
299 { UBLOX_U8, offsetof(struct nav_svinfo_sat, chn) }, /* 8 + 12*N chn */
300 { UBLOX_U8, offsetof(struct nav_svinfo_sat, svid) }, /* 9 + 12*N svid */
301 { UBLOX_U8, offsetof(struct nav_svinfo_sat, flags) }, /* 10 + 12*N flags */
302 { UBLOX_U8, offsetof(struct nav_svinfo_sat, quality) }, /* 11 + 12*N quality */
303 { UBLOX_U8, offsetof(struct nav_svinfo_sat, cno) }, /* 12 + 12*N cno */
304 { UBLOX_DISCARD, 7 }, /* 13 + 12*N elev, azim, prRes */
308 #define NAV_SVINFO_SAT_FLAGS_SVUSED 0
309 #define NAV_SVINFO_SAT_FLAGS_DIFFCORR 1
310 #define NAV_SVINFO_SAT_FLAGS_ORBITAVAIL 2
311 #define NAV_SVINFO_SAT_FLAGS_ORBITEPH 3
312 #define NAV_SVINFO_SAT_FLAGS_UNHEALTHY 4
313 #define NAV_SVINFO_SAT_FLAGS_ORBITALM 5
314 #define NAV_SVINFO_SAT_FLAGS_ORBITAOP 6
315 #define NAV_SVINFO_SAT_FLAGS_SMOOTHED 7
317 #define NAV_SVINFO_SAT_QUALITY_IDLE 0
318 #define NAV_SVINFO_SAT_QUALITY_SEARCHING 1
319 #define NAV_SVINFO_SAT_QUALITY_ACQUIRED 2
320 #define NAV_SVINFO_SAT_QUALITY_UNUSABLE 3
321 #define NAV_SVINFO_SAT_QUALITY_LOCKED 4
322 #define NAV_SVINFO_SAT_QUALITY_RUNNING 5
325 ao_ublox_parse_nav_svinfo(void)
329 ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
330 for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
331 if (nsat < NAV_SVINFO_MAX_SAT) {
332 ao_ublox_parse(&nav_svinfo_sat[nav_svinfo_nsat++], nav_svinfo_sat_packet);
340 * NAV-TIMEUTC message parsing
342 static struct nav_timeutc {
352 #define NAV_TIMEUTC_VALID_TOW 0
353 #define NAV_TIMEUTC_VALID_WKN 1
354 #define NAV_TIMEUTC_VALID_UTC 2
356 static const struct ublox_packet_parse nav_timeutc_packet[] = {
357 { UBLOX_DISCARD, 12 }, /* 0 iTOW, tAcc, nano */
358 { UBLOX_U16, offsetof(struct nav_timeutc, year) }, /* 12 year */
359 { UBLOX_U8, offsetof(struct nav_timeutc, month) }, /* 14 month */
360 { UBLOX_U8, offsetof(struct nav_timeutc, day) }, /* 15 day */
361 { UBLOX_U8, offsetof(struct nav_timeutc, hour) }, /* 16 hour */
362 { UBLOX_U8, offsetof(struct nav_timeutc, min) }, /* 17 min */
363 { UBLOX_U8, offsetof(struct nav_timeutc, sec) }, /* 18 sec */
364 { UBLOX_U8, offsetof(struct nav_timeutc, valid) }, /* 19 valid */
369 ao_ublox_parse_nav_timeutc(void)
371 ao_ublox_parse(&nav_timeutc, nav_timeutc_packet);
375 * NAV-VELNED message parsing
378 static struct nav_velned {
384 static const struct ublox_packet_parse nav_velned_packet[] = {
385 { UBLOX_DISCARD, 12 }, /* 0 iTOW, velN, velE */
386 { UBLOX_U32, offsetof(struct nav_velned, vel_d) }, /* 12 velD */
387 { UBLOX_DISCARD, 4 }, /* 16 speed */
388 { UBLOX_U32, offsetof(struct nav_velned, g_speed) }, /* 20 gSpeed */
389 { UBLOX_U32, offsetof(struct nav_velned, heading) }, /* 24 heading */
390 { UBLOX_DISCARD, 8 }, /* 28 sAcc, cAcc */
395 ao_ublox_parse_nav_velned(void)
397 ao_ublox_parse(&nav_velned, nav_velned_packet);
401 * Set the protocol mode and baud rate
408 ao_ublox_set_speed(AO_SERIAL_SPEED_9600);
411 * A bunch of nulls so the start bit
414 for (i = 0; i < 64; i++)
415 ao_ublox_putchar(0x00);
418 * Send the baud-rate setting and protocol-setting
419 * command three times
421 for (k = 0; k < 3; k++)
422 for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
423 ao_ublox_putchar(ao_gps_set_nmea[i]);
426 * Increase the baud rate
428 ao_ublox_set_speed(AO_SERIAL_SPEED_57600);
431 * Pad with nulls to give the chip
432 * time to see the baud rate switch
434 for (i = 0; i < 64; i++)
435 ao_ublox_putchar(0x00);
439 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
441 ao_ublox_init_cksum();
442 ao_ublox_putchar(0xb5);
443 ao_ublox_putchar(0x62);
444 ao_ublox_put_u8(class);
446 ao_ublox_put_u8(len);
447 ao_ublox_put_u8(len >> 8);
451 ao_ublox_putend(void)
453 ao_ublox_putchar(ao_ublox_cksum.a);
454 ao_ublox_putchar(ao_ublox_cksum.b);
458 ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
460 ao_ublox_putstart(0x06, 0x01, 3);
461 ao_ublox_put_u8(class);
462 ao_ublox_put_u8(msgid);
463 ao_ublox_put_u8(rate);
468 ao_ublox_set_navigation_settings(uint16_t mask,
472 uint32_t fixed_alt_var,
479 uint8_t static_hold_thresh,
480 uint8_t dgps_time_out)
482 ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
483 ao_ublox_put_u16(mask);
484 ao_ublox_put_u8(dyn_model);
485 ao_ublox_put_u8(fix_mode);
486 ao_ublox_put_i32(fixed_alt);
487 ao_ublox_put_u32(fixed_alt_var);
488 ao_ublox_put_i8(min_elev);
489 ao_ublox_put_u8(dr_limit);
490 ao_ublox_put_u16(pdop);
491 ao_ublox_put_u16(tdop);
492 ao_ublox_put_u16(pacc);
493 ao_ublox_put_u16(tacc);
494 ao_ublox_put_u8(static_hold_thresh);
495 ao_ublox_put_u8(dgps_time_out);
504 * Disable all MON message
506 static const uint8_t ublox_disable_mon[] = {
507 0x0b, 0x09, 0x02, 0x06, 0x07, 0x21, 0x08, 0x04
511 * Disable all NAV messages. The desired
512 * ones will be explicitly re-enabled
515 static const uint8_t ublox_disable_nav[] = {
516 0x60, 0x22, 0x31, 0x04, 0x40, 0x01, 0x02, 0x32,
517 0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12
521 * Enable enough messages to get all of the data we want
523 static const uint8_t ublox_enable_nav[] = {
533 ao_gps(void) __reentrant
536 struct ao_ublox_cksum cksum;
541 /* Disable all messages */
542 for (i = 0; i < sizeof (ublox_disable_mon); i++)
543 ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0);
544 for (i = 0; i < sizeof (ublox_disable_nav); i++)
545 ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
547 /* Enable all of the messages we want */
548 for (i = 0; i < sizeof (ublox_enable_nav); i++)
549 ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
551 ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
552 UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
553 UBLOX_CFG_NAV5_FIXMODE_3D,
566 /* Locate the begining of the next record */
567 while (ao_ublox_byte() != (uint8_t) 0xb5)
569 if (ao_ublox_byte() != (uint8_t) 0x62)
572 ao_ublox_init_cksum();
574 class = header_byte();
578 ao_ublox_len = header_byte();
579 ao_ublox_len |= header_byte() << 8;
581 if (ao_ublox_len > 1023)
588 if (ao_ublox_len != 18)
590 ao_ublox_parse_nav_dop();
592 case UBLOX_NAV_POSLLH:
593 if (ao_ublox_len != 28)
595 ao_ublox_parse_nav_posllh();
598 if (ao_ublox_len != 52)
600 ao_ublox_parse_nav_sol();
602 case UBLOX_NAV_SVINFO:
603 if (ao_ublox_len < 8)
605 ao_ublox_parse_nav_svinfo();
607 case UBLOX_NAV_VELNED:
608 if (ao_ublox_len != 36)
610 ao_ublox_parse_nav_velned();
612 case UBLOX_NAV_TIMEUTC:
613 if (ao_ublox_len != 20)
615 ao_ublox_parse_nav_timeutc();
621 if (ao_ublox_len != 0)
624 /* verify checksum and end sequence */
625 cksum.a = ao_ublox_byte();
626 cksum.b = ao_ublox_byte();
627 if (ao_ublox_cksum.a != cksum.a || ao_ublox_cksum.b != cksum.b)
634 ao_mutex_get(&ao_gps_mutex);
635 ao_gps_tick = ao_time();
637 ao_gps_data.flags = 0;
638 ao_gps_data.flags |= AO_GPS_RUNNING;
639 if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
640 uint8_t nsat = nav_sol.nsat;
641 ao_gps_data.flags |= AO_GPS_VALID;
644 ao_gps_data.flags |= nsat;
646 if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
647 ao_gps_data.flags |= AO_GPS_DATE_VALID;
649 ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
650 ao_gps_data.latitude = nav_posllh.lat;
651 ao_gps_data.longitude = nav_posllh.lon;
653 ao_gps_data.year = nav_timeutc.year - 2000;
654 ao_gps_data.month = nav_timeutc.month;
655 ao_gps_data.day = nav_timeutc.day;
657 ao_gps_data.hour = nav_timeutc.hour;
658 ao_gps_data.minute = nav_timeutc.min;
659 ao_gps_data.second = nav_timeutc.sec;
661 ao_gps_data.pdop = nav_dop.pdop;
662 ao_gps_data.hdop = nav_dop.hdop;
663 ao_gps_data.vdop = nav_dop.vdop;
665 /* mode is not set */
667 ao_gps_data.ground_speed = nav_velned.g_speed;
668 ao_gps_data.climb_rate = -nav_velned.vel_d;
669 ao_gps_data.course = nav_velned.heading / 200000;
671 ao_gps_tracking_data.channels = 0;
673 struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
675 for (i = 0; i < nav_svinfo_nsat; i++) {
676 struct nav_svinfo_sat *src = &nav_svinfo_sat[i];
678 if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
679 src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
681 dst->svid = src->svid;
682 dst->c_n_1 = src->cno;
684 ao_gps_tracking_data.channels++;
688 ao_mutex_put(&ao_gps_mutex);
689 ao_wakeup(&ao_gps_data);
690 ao_wakeup(&ao_gps_tracking_data);
698 __xdata struct ao_task ao_gps_task;
703 ao_add_task(&ao_gps_task, ao_gps, "gps");