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; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
23 #include "ao_gps_ublox.h"
25 #define AO_UBLOX_DEBUG 0
27 #ifndef AO_UBLOX_VERSION
28 #define AO_UBLOX_VERSION 8
35 AO_TICK_TYPE ao_gps_tick;
36 AO_TICK_TYPE ao_gps_utc_tick;
37 struct ao_telemetry_location ao_gps_data;
38 struct ao_telemetry_satellite ao_gps_tracking_data;
40 #undef AO_SERIAL_SPEED_UBLOX
42 #ifndef AO_SERIAL_SPEED_UBLOX
43 #define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600
46 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
47 #define SERIAL_SPEED_STRING "57600"
48 #define SERIAL_SPEED_CHECKSUM "2d"
50 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
51 #define SERIAL_SPEED_STRING "19200"
52 #define SERIAL_SPEED_CHECKSUM "23"
54 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
55 #define SERIAL_SPEED_STRING "9600"
56 #define SERIAL_SPEED_CHECKSUM "16"
59 static const char ao_gps_set_nmea[] =
60 "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
62 struct ao_ublox_cksum {
66 static struct ao_ublox_cksum ao_ublox_cksum;
67 static uint16_t ao_ublox_len;
76 static uint8_t ao_gps_dbg_enable = 0;
78 static void ao_gps_dbg(int level, char *format, ...) {
81 if (level & ao_gps_dbg_enable) {
90 #define ao_gps_dbg(fmt, ...)
93 static inline uint8_t ao_ublox_byte(void) {
94 uint8_t c = (uint8_t) ao_gps_getchar();
95 ao_gps_dbg(DBG_CHAR, " %02x", c);
99 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
102 cksum->b += cksum->a;
105 static void ao_ublox_init_cksum(void)
107 ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
110 static void ao_ublox_put_u8(uint8_t c)
112 add_cksum(&ao_ublox_cksum, c);
113 ao_gps_dbg(DBG_CHAR, " (%02x)", c);
117 static void ao_ublox_put_i8(int8_t c)
119 ao_ublox_put_u8((uint8_t) c);
122 static void ao_ublox_put_u16(uint16_t c)
124 ao_ublox_put_u8((uint8_t) c);
125 ao_ublox_put_u8((uint8_t) (c>>8));
129 static void ao_ublox_put_i16(int16_t c)
131 ao_ublox_put_u16((uint16_t) c);
135 static void ao_ublox_put_u32(uint32_t c)
137 ao_ublox_put_u8((uint8_t) c);
138 ao_ublox_put_u8((uint8_t) (c>>8));
139 ao_ublox_put_u8((uint8_t) (c>>16));
140 ao_ublox_put_u8((uint8_t) (c>>24));
143 static void ao_ublox_put_i32(int32_t c)
145 ao_ublox_put_u32((uint32_t) c);
148 static uint8_t header_byte(void)
150 uint8_t c = ao_ublox_byte();
151 add_cksum(&ao_ublox_cksum, c);
155 static uint8_t data_byte(void)
158 return header_byte();
161 static char *ublox_target;
163 static void ublox_u16(uint8_t offset)
165 uint16_t *ptr = (uint16_t *) (void *) (ublox_target + offset);
169 val |= (uint16_t) ((uint16_t) data_byte () << 8);
173 static void ublox_u8(uint8_t offset)
175 uint8_t *ptr = (uint8_t *) (ublox_target + offset);
182 static void ublox_u32(uint8_t offset)
184 uint32_t *ptr = (uint32_t *) (void *) (ublox_target + offset);
187 val = ((uint32_t) data_byte ());
188 val |= ((uint32_t) data_byte ()) << 8;
189 val |= ((uint32_t) data_byte ()) << 16;
190 val |= ((uint32_t) data_byte ()) << 24;
194 static void ublox_discard(uint8_t len)
201 #define UBLOX_DISCARD 1
206 struct ublox_packet_parse {
212 ao_ublox_parse(void *target, const struct ublox_packet_parse *parse)
216 ublox_target = target;
218 offset = parse[i].offset;
219 switch (parse[i].type) {
223 ublox_discard(offset);
236 ao_gps_dbg(DBG_PROTO, "\n");
240 * NAV-DOP message parsing
243 static struct nav_dop {
249 static const struct ublox_packet_parse nav_dop_packet[] = {
250 { UBLOX_DISCARD, 6 }, /* 0 GPS Millisecond Time of Week, gDOP */
251 { UBLOX_U16, offsetof(struct nav_dop, pdop) }, /* 6 pDOP */
252 { UBLOX_DISCARD, 2 }, /* 8 tDOP */
253 { UBLOX_U16, offsetof(struct nav_dop, vdop) }, /* 10 vDOP */
254 { UBLOX_U16, offsetof(struct nav_dop, hdop) }, /* 12 hDOP */
255 { UBLOX_DISCARD, 4 }, /* 14 nDOP, eDOP */
260 ao_ublox_parse_nav_dop(void)
262 ao_ublox_parse(&nav_dop, nav_dop_packet);
265 static const struct ublox_packet_parse ack_ack_packet[] = {
266 { UBLOX_DISCARD, 2 }, /* 0 class ID, msg ID */
271 ao_ublox_parse_ack_ack(void)
273 ao_ublox_parse(NULL, ack_ack_packet);
276 static struct ack_nak {
281 static const struct ublox_packet_parse ack_nak_packet[] = {
282 { UBLOX_U8, offsetof(struct ack_nak, class_id) }, /* 0 class ID */
283 { UBLOX_U8, offsetof(struct ack_nak, msg_id) }, /* 1 msg ID */
288 ao_ublox_parse_ack_nak(void)
290 ao_ublox_parse(&ack_nak, ack_nak_packet);
292 ao_gps_dbg(DBG_PROTO, "NAK class 0x%02x msg 0x%02x\n",
293 ack_nak.class_id, ack_nak.msg_id);
297 #if AO_UBLOX_VERSION < 10
299 * NAV-POSLLH message parsing
301 static struct nav_posllh {
307 static const struct ublox_packet_parse nav_posllh_packet[] = {
308 { UBLOX_DISCARD, 4 }, /* 0 GPS Millisecond Time of Week */
309 { UBLOX_U32, offsetof(struct nav_posllh, lon) }, /* 4 Longitude */
310 { UBLOX_U32, offsetof(struct nav_posllh, lat) }, /* 8 Latitude */
311 { UBLOX_DISCARD, 4 }, /* 12 Height above Ellipsoid */
312 { UBLOX_U32, offsetof(struct nav_posllh, alt_msl) }, /* 16 Height above mean sea level */
313 { UBLOX_DISCARD, 8 }, /* 20 hAcc, vAcc */
314 { UBLOX_END, 0 }, /* 28 */
318 ao_ublox_parse_nav_posllh(void)
320 ao_ublox_parse(&nav_posllh, nav_posllh_packet);
324 * NAV-SOL message parsing
326 static struct nav_sol {
332 static const struct ublox_packet_parse nav_sol_packet[] = {
333 { UBLOX_DISCARD, 10 }, /* 0 iTOW, fTOW, week */
334 { UBLOX_U8, offsetof(struct nav_sol, gps_fix) }, /* 10 gpsFix */
335 { UBLOX_U8, offsetof(struct nav_sol, flags) }, /* 11 flags */
336 { UBLOX_DISCARD, 35 }, /* 12 ecefX, ecefY, ecefZ, pAcc, ecefVX, ecefVY, ecefVZ, sAcc, pDOP, reserved1 */
337 { UBLOX_U8, offsetof(struct nav_sol, nsat) }, /* 47 numSV */
338 { UBLOX_DISCARD, 4 }, /* 48 reserved2 */
342 #define NAV_SOL_FLAGS_GPSFIXOK 0
343 #define NAV_SOL_FLAGS_DIFFSOLN 1
344 #define NAV_SOL_FLAGS_WKNSET 2
345 #define NAV_SOL_FLAGS_TOWSET 3
348 ao_ublox_parse_nav_sol(void)
350 ao_ublox_parse(&nav_sol, nav_sol_packet);
354 * NAV-SVINFO message parsing
357 static struct nav_svinfo {
362 static const struct ublox_packet_parse nav_svinfo_packet[] = {
363 { UBLOX_DISCARD, 4 }, /* 0 iTOW */
364 { UBLOX_U8, offsetof(struct nav_svinfo, num_ch) }, /* 4 numCh */
365 { UBLOX_U8, offsetof(struct nav_svinfo, flags) }, /* 5 globalFlags */
366 { UBLOX_DISCARD, 2 }, /* 6 reserved2 */
370 #define NAV_SVINFO_MAX_SAT 16
372 static struct nav_svinfo_sat {
378 } nav_svinfo_sat[NAV_SVINFO_MAX_SAT];
380 static uint8_t nav_svinfo_nsat;
382 static const struct ublox_packet_parse nav_svinfo_sat_packet[] = {
383 { UBLOX_U8, offsetof(struct nav_svinfo_sat, chn) }, /* 8 + 12*N chn */
384 { UBLOX_U8, offsetof(struct nav_svinfo_sat, svid) }, /* 9 + 12*N svid */
385 { UBLOX_U8, offsetof(struct nav_svinfo_sat, flags) }, /* 10 + 12*N flags */
386 { UBLOX_U8, offsetof(struct nav_svinfo_sat, quality) }, /* 11 + 12*N quality */
387 { UBLOX_U8, offsetof(struct nav_svinfo_sat, cno) }, /* 12 + 12*N cno */
388 { UBLOX_DISCARD, 7 }, /* 13 + 12*N elev, azim, prRes */
392 #define NAV_SVINFO_SAT_FLAGS_SVUSED 0
393 #define NAV_SVINFO_SAT_FLAGS_DIFFCORR 1
394 #define NAV_SVINFO_SAT_FLAGS_ORBITAVAIL 2
395 #define NAV_SVINFO_SAT_FLAGS_ORBITEPH 3
396 #define NAV_SVINFO_SAT_FLAGS_UNHEALTHY 4
397 #define NAV_SVINFO_SAT_FLAGS_ORBITALM 5
398 #define NAV_SVINFO_SAT_FLAGS_ORBITAOP 6
399 #define NAV_SVINFO_SAT_FLAGS_SMOOTHED 7
401 #define NAV_SVINFO_SAT_QUALITY_IDLE 0
402 #define NAV_SVINFO_SAT_QUALITY_SEARCHING 1
403 #define NAV_SVINFO_SAT_QUALITY_ACQUIRED 2
404 #define NAV_SVINFO_SAT_QUALITY_UNUSABLE 3
405 #define NAV_SVINFO_SAT_QUALITY_LOCKED 4
406 #define NAV_SVINFO_SAT_QUALITY_RUNNING 5
409 ao_ublox_parse_nav_svinfo(void)
414 ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
415 for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
416 if (nsat < NAV_SVINFO_MAX_SAT) {
417 ao_ublox_parse(&nav_svinfo_sat[nav_svinfo_nsat++], nav_svinfo_sat_packet);
423 ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
424 for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
425 ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
427 nav_svinfo_sat[nsat].chn,
428 nav_svinfo_sat[nsat].svid,
429 nav_svinfo_sat[nsat].flags,
430 nav_svinfo_sat[nsat].quality,
431 nav_svinfo_sat[nsat].cno);
436 * NAV-TIMEUTC message parsing
438 static struct nav_timeutc {
449 #define NAV_TIMEUTC_VALID_TOW 0
450 #define NAV_TIMEUTC_VALID_WKN 1
451 #define NAV_TIMEUTC_VALID_UTC 2
453 static const struct ublox_packet_parse nav_timeutc_packet[] = {
454 { UBLOX_DISCARD, 8 }, /* 0 iTOW, tAcc */
455 { UBLOX_U32, offsetof(struct nav_timeutc, nano) }, /* 8 nano */
456 { UBLOX_U16, offsetof(struct nav_timeutc, year) }, /* 12 year */
457 { UBLOX_U8, offsetof(struct nav_timeutc, month) }, /* 14 month */
458 { UBLOX_U8, offsetof(struct nav_timeutc, day) }, /* 15 day */
459 { UBLOX_U8, offsetof(struct nav_timeutc, hour) }, /* 16 hour */
460 { UBLOX_U8, offsetof(struct nav_timeutc, min) }, /* 17 min */
461 { UBLOX_U8, offsetof(struct nav_timeutc, sec) }, /* 18 sec */
462 { UBLOX_U8, offsetof(struct nav_timeutc, valid) }, /* 19 valid */
467 ao_ublox_parse_nav_timeutc(void)
469 ao_ublox_parse(&nav_timeutc, nav_timeutc_packet);
473 * NAV-VELNED message parsing
476 static struct nav_velned {
482 static const struct ublox_packet_parse nav_velned_packet[] = {
483 { UBLOX_DISCARD, 12 }, /* 0 iTOW, velN, velE */
484 { UBLOX_U32, offsetof(struct nav_velned, vel_d) }, /* 12 velD */
485 { UBLOX_DISCARD, 4 }, /* 16 speed */
486 { UBLOX_U32, offsetof(struct nav_velned, g_speed) }, /* 20 gSpeed */
487 { UBLOX_U32, offsetof(struct nav_velned, heading) }, /* 24 heading */
488 { UBLOX_DISCARD, 8 }, /* 28 sAcc, cAcc */
493 ao_ublox_parse_nav_velned(void)
495 ao_ublox_parse(&nav_velned, nav_velned_packet);
498 #else /* AO_UBLOX_VERSION < 10 */
501 * NAV-PVT message parsing
504 static struct nav_pvt {
523 static const struct ublox_packet_parse nav_pvt_packet[] = {
524 { UBLOX_DISCARD, 4 }, /* 0 iTOW */
525 { UBLOX_U16, offsetof(struct nav_pvt, year) }, /* 4 year */
526 { UBLOX_U8, offsetof(struct nav_pvt, month) }, /* 6 month */
527 { UBLOX_U8, offsetof(struct nav_pvt, day) }, /* 7 day */
528 { UBLOX_U8, offsetof(struct nav_pvt, hour) }, /* 8 hour */
529 { UBLOX_U8, offsetof(struct nav_pvt, min) }, /* 9 min */
530 { UBLOX_U8, offsetof(struct nav_pvt, sec) }, /* 10 sec */
531 { UBLOX_U8, offsetof(struct nav_pvt, valid) }, /* 11 valid */
532 { UBLOX_DISCARD, 4 }, /* 12 tAcc */
533 { UBLOX_U32, offsetof(struct nav_pvt, nano) }, /* 16 nano */
534 { UBLOX_DISCARD, 1 }, /* 20 fixType */
535 { UBLOX_U8, offsetof(struct nav_pvt, flags) }, /* 21 gpsFix */
536 { UBLOX_DISCARD, 1 }, /* 22 flags2 */
537 { UBLOX_U8, offsetof(struct nav_pvt, num_sv) }, /* 23 numSV */
538 { UBLOX_U32, offsetof(struct nav_pvt, lon) }, /* 24 Longitude */
539 { UBLOX_U32, offsetof(struct nav_pvt, lat) }, /* 28 Latitude */
540 { UBLOX_DISCARD, 4 }, /* 32 height above ellipsoid */
541 { UBLOX_U32, offsetof(struct nav_pvt, alt_msl) }, /* 36 Height above mean sea level */
542 { UBLOX_DISCARD, 16 }, /* 40 hAcc, vAcc, velN, velE */
543 { UBLOX_U32, offsetof(struct nav_pvt, vel_d) }, /* 56 velD */
544 { UBLOX_U32, offsetof(struct nav_pvt, g_speed) }, /* 60 gSpeed */
545 { UBLOX_U32, offsetof(struct nav_pvt, heading) }, /* 64 headMot */
546 { UBLOX_DISCARD, 92 - 68 }, /* 68 sAcc .. magAcc */
550 #define NAV_PVT_VALID_DATE 0
551 #define NAV_PVT_VALID_TIME 1
552 #define NAV_PVT_VALID_FULLY_RESOLVED 2
553 #define NAV_PVT_VALID_MAG 3
555 #define NAV_PVT_FLAGS_GNSSFIXOK 0
556 #define NAV_PVT_FLAGS_DIFFSOLN 1
557 #define NAV_PVT_FLAGS_PSM_STATE 2
558 #define NAV_PVT_FLAGS_HEAD_VEH_VALID 5
559 #define NAV_PVT_FLAGS_CARR_SOLN 6
562 ao_ublox_parse_nav_pvt(void)
564 ao_ublox_parse(&nav_pvt, nav_pvt_packet);
566 ao_gps_dbg(DBG_PROTO, "\t%d-%d-%d %02d:%02d:%02d %ld (%02x)\n",
567 nav_pvt.year, nav_pvt.month, nav_pvt.day,
568 nav_pvt.hour, nav_pvt.min, nav_pvt.sec,
569 (long) nav_pvt.nano, nav_pvt.valid);
570 ao_gps_dbg(DBG_PROTO, "\tflags %02x numSV %d lon %ld lat %ld alt %ld\n",
571 nav_pvt.flags, nav_pvt.num_sv,
572 (long) nav_pvt.lon, (long) nav_pvt.lat, (long) nav_pvt.alt_msl);
577 * NAV-SAT message parsing
580 static struct nav_sat {
584 static const struct ublox_packet_parse nav_sat_packet[] = {
585 { UBLOX_DISCARD, 4 }, /* 0 iTOW */
586 { UBLOX_DISCARD, 1 }, /* 4 version */
587 { UBLOX_U8, offsetof(struct nav_sat, num_svs) }, /* 4 numSvs */
588 { UBLOX_DISCARD, 2 }, /* 6 reserved0 */
592 #define NAV_SAT_MAX_SAT AO_TELEMETRY_SATELLITE_MAX_SAT
594 static struct nav_sat_sat {
597 } nav_sat_sat[NAV_SAT_MAX_SAT];
599 static uint8_t nav_sat_nsat;
601 static struct nav_sat_real_sat {
607 static const struct ublox_packet_parse nav_sat_sat_packet[] = {
608 { UBLOX_DISCARD, 1 }, /* 8 + 12*N gnssid */
609 { UBLOX_U8, offsetof(struct nav_sat_real_sat, svid) }, /* 9 + 12*N svid */
610 { UBLOX_U8, offsetof(struct nav_sat_real_sat, cno) }, /* 10 + 12*N cno */
611 { UBLOX_DISCARD, 5 }, /* 11 + 12*N elev, azim, prRes */
612 { UBLOX_U32, offsetof(struct nav_sat_real_sat, flags) }, /* 16 + 12*N flags */
617 ao_ublox_sat_quality(struct nav_sat_real_sat *sat)
619 return (sat->flags >> UBLOX_NAV_SAT_FLAGS_QUALITY) & UBLOX_NAV_SAT_FLAGS_QUALITY_MASK;
623 ao_ublox_sat_health(struct nav_sat_real_sat *sat)
625 return (sat->flags >> UBLOX_NAV_SAT_FLAGS_SV_HEALTH) & UBLOX_NAV_SAT_FLAGS_SV_HEALTH_MASK;
629 ao_ublox_parse_nav_sat(void)
634 ao_ublox_parse(&nav_sat, nav_sat_packet);
635 for (nsat = 0; nsat < nav_sat.num_svs && ao_ublox_len >= 12; nsat++) {
636 ao_ublox_parse(&nav_sat_real_sat, nav_sat_sat_packet);
637 if (nav_sat_nsat < NAV_SAT_MAX_SAT &&
638 ao_ublox_sat_health(&nav_sat_real_sat) == UBLOX_NAV_SAT_FLAGS_SV_HEALTH_HEALTHY &&
639 ao_ublox_sat_quality(&nav_sat_real_sat) >= UBLOX_NAV_SAT_FLAGS_QUALITY_ACQUIRED)
641 nav_sat_sat[nav_sat_nsat].svid = nav_sat_real_sat.svid;
642 nav_sat_sat[nav_sat_nsat].cno = nav_sat_real_sat.cno;
647 ao_gps_dbg(DBG_PROTO, "sat num_svs %d\n", nav_sat.num_svs);
648 for (nsat = 0; nsat < nav_sat.num_svs; nsat++) {
649 if (nsat < NAV_SAT_MAX_SAT) {
650 ao_gps_dbg(DBG_PROTO, "\t%d: svid %d cno %d\n",
652 nav_sat_sat[nsat].svid,
653 nav_sat_sat[nsat].cno);
655 ao_gps_dbg(DBG_PROTO, "\t%d: skipped\n", nsat);
661 #endif /* else AO_UBLOX_VERSION < 10 */
664 * Set the protocol mode and baud rate
673 * A bunch of nulls so the start bit
677 for (i = 0; i < 64; i++)
678 ao_gps_putchar(0x00);
686 ao_delay(AO_SEC_TO_TICKS(3));
688 ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
689 ao_gps_set_speed(AO_SERIAL_SPEED_9600);
692 * Send the baud-rate setting and protocol-setting
693 * command three times
695 for (k = 0; k < 3; k++) {
698 ao_gps_dbg(DBG_INIT, "Send initial setting\n");
699 for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
700 ao_gps_putchar(ao_gps_set_nmea[i]);
705 #if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
706 ao_gps_dbg(DBG_INIT, "Set speed high\n");
708 * Increase the baud rate
710 ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
717 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
719 ao_ublox_init_cksum();
720 ao_gps_putchar(0xb5);
721 ao_gps_putchar(0x62);
722 ao_ublox_put_u8(class);
724 ao_ublox_put_u8((uint8_t) len);
725 ao_ublox_put_u8((uint8_t) (len >> 8));
729 ao_ublox_putend(void)
731 ao_gps_putchar(ao_ublox_cksum.a);
732 ao_gps_putchar(ao_ublox_cksum.b);
736 ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
738 ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_MSG, 3);
739 ao_ublox_put_u8(class);
740 ao_ublox_put_u8(msgid);
741 ao_ublox_put_u8(rate);
746 ao_ublox_set_navigation_settings(uint16_t mask,
750 uint32_t fixed_alt_var,
757 uint8_t static_hold_thresh,
758 uint8_t dgps_time_out)
760 ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
761 ao_ublox_put_u16(mask);
762 ao_ublox_put_u8(dyn_model);
763 ao_ublox_put_u8(fix_mode);
764 ao_ublox_put_i32(fixed_alt);
765 ao_ublox_put_u32(fixed_alt_var);
766 ao_ublox_put_i8(min_elev);
767 ao_ublox_put_u8(dr_limit);
768 ao_ublox_put_u16(pdop);
769 ao_ublox_put_u16(tdop);
770 ao_ublox_put_u16(pacc);
771 ao_ublox_put_u16(tacc);
772 ao_ublox_put_u8(static_hold_thresh);
773 ao_ublox_put_u8(dgps_time_out);
781 * Disable all MON message
783 static const uint8_t ublox_disable_mon[] = {
784 0x0b, 0x09, 0x02, 0x06, 0x07, 0x21, 0x08, 0x04
788 * Disable all NAV messages. The desired
789 * ones will be explicitly re-enabled
792 static const uint8_t ublox_disable_nav[] = {
793 0x60, 0x22, 0x31, 0x04, 0x40, 0x01, 0x02, 0x32,
794 0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12
798 * Enable enough messages to get all of the data we want
800 static const uint8_t ublox_enable_nav[] = {
801 UBLOX_NAV_DOP, /* both */
802 #if AO_UBLOX_VERSION >= 10
803 UBLOX_NAV_PVT, /* new */
804 UBLOX_NAV_SAT, /* new */
806 UBLOX_NAV_POSLLH, /* both, but redundant with PVT */
807 UBLOX_NAV_SOL, /* old */
808 UBLOX_NAV_SVINFO, /* old */
809 UBLOX_NAV_VELNED, /* both, but redundant with PVT */
810 UBLOX_NAV_TIMEUTC /* both, but redundant with PVT */
815 ao_gps_set_rate(uint8_t rate)
818 for (i = 0; i < sizeof (ublox_enable_nav); i++)
819 ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate);
826 struct ao_ublox_cksum cksum;
828 AO_TICK_TYPE packet_start_tick;
829 AO_TICK_TYPE solution_tick = 0;
833 /* Disable all messages */
834 for (i = 0; i < sizeof (ublox_disable_mon); i++)
835 ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0);
836 for (i = 0; i < sizeof (ublox_disable_nav); i++)
837 ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
839 /* Enable all of the messages we want */
842 ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
843 UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
844 UBLOX_CFG_NAV5_FIXMODE_3D,
857 /* Locate the begining of the next record */
858 while (ao_ublox_byte() != (uint8_t) 0xb5)
860 packet_start_tick = ao_tick_count;
862 if (ao_ublox_byte() != (uint8_t) 0x62)
865 ao_ublox_init_cksum();
867 class = header_byte();
871 ao_ublox_len = header_byte();
872 ao_ublox_len |= (uint16_t) ((uint16_t) header_byte() << 8);
874 ao_gps_dbg(DBG_PROTO, "%6u class %02x id %02x len %d\n", packet_start_tick, class, id, ao_ublox_len);
876 if (ao_ublox_len > 1023)
879 bool gps_ready = false;
885 if (ao_ublox_len != 18)
887 ao_ublox_parse_nav_dop();
888 #if AO_UBLOX_VERSION >= 10
892 #if AO_UBLOX_VERSION >= 10
894 if (ao_ublox_len != 92)
896 ao_ublox_parse_nav_pvt();
897 solution_tick = packet_start_tick;
900 if (ao_ublox_len < 8)
902 ao_ublox_parse_nav_sat();
905 case UBLOX_NAV_POSLLH:
906 if (ao_ublox_len != 28)
908 ao_ublox_parse_nav_posllh();
911 if (ao_ublox_len != 52)
913 ao_ublox_parse_nav_sol();
914 solution_tick = packet_start_tick;
916 case UBLOX_NAV_SVINFO:
917 if (ao_ublox_len < 8)
919 ao_ublox_parse_nav_svinfo();
921 case UBLOX_NAV_VELNED:
922 if (ao_ublox_len != 36)
924 ao_ublox_parse_nav_velned();
926 case UBLOX_NAV_TIMEUTC:
927 if (ao_ublox_len != 20)
929 ao_ublox_parse_nav_timeutc();
938 if (ao_ublox_len != 2)
940 ao_ublox_parse_ack_ack();
943 if (ao_ublox_len != 2)
945 ao_ublox_parse_ack_nak();
950 if (ao_ublox_len != 0) {
951 ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
955 /* verify checksum and end sequence */
956 cksum.a = ao_ublox_byte();
957 cksum.b = ao_ublox_byte();
958 if (ao_ublox_cksum.a != cksum.a || ao_ublox_cksum.b != cksum.b)
964 ao_mutex_get(&ao_gps_mutex);
965 ao_gps_data.flags = 0;
966 ao_gps_data.flags |= AO_GPS_RUNNING;
967 ao_gps_tick = solution_tick;
969 /* we report dop scaled by 10, but ublox provides dop scaled by 100
971 ao_gps_data.pdop = (uint8_t) (nav_dop.pdop / 10);
972 ao_gps_data.hdop = (uint8_t) (nav_dop.hdop / 10);
973 ao_gps_data.vdop = (uint8_t) (nav_dop.vdop / 10);
975 #if AO_UBLOX_VERSION >= 10
976 ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_pvt.nano);
977 if (nav_pvt.flags & (1 << NAV_PVT_FLAGS_GNSSFIXOK)) {
978 uint8_t nsat = nav_pvt.num_sv;
979 ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
982 ao_gps_data.flags |= nsat;
984 if (nav_pvt.valid & (1 << NAV_PVT_VALID_DATE))
985 ao_gps_data.flags |= AO_GPS_DATE_VALID;
986 AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_pvt.alt_msl / 1000);
987 ao_gps_data.latitude = nav_pvt.lat;
988 ao_gps_data.longitude = nav_pvt.lon;
990 ao_gps_data.year = (uint8_t) (nav_pvt.year - 2000);
991 ao_gps_data.month = nav_pvt.month;
992 ao_gps_data.day = nav_pvt.day;
994 ao_gps_data.hour = nav_pvt.hour;
995 ao_gps_data.minute = nav_pvt.min;
996 ao_gps_data.second = nav_pvt.sec;
998 /* ublox speeds are mm/s */
999 ao_gps_data.ground_speed = (uint16_t) (nav_pvt.g_speed / 10);
1000 ao_gps_data.climb_rate = -(int16_t) (nav_pvt.vel_d / 10);
1001 ao_gps_data.course = (uint8_t) (nav_pvt.heading / 200000);
1003 ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_timeutc.nano);
1004 if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
1005 uint8_t nsat = nav_sol.nsat;
1006 ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
1009 ao_gps_data.flags |= nsat;
1011 if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
1012 ao_gps_data.flags |= AO_GPS_DATE_VALID;
1014 AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000);
1015 ao_gps_data.latitude = nav_posllh.lat;
1016 ao_gps_data.longitude = nav_posllh.lon;
1018 ao_gps_data.year = (uint8_t) (nav_timeutc.year - 2000);
1019 ao_gps_data.month = nav_timeutc.month;
1020 ao_gps_data.day = nav_timeutc.day;
1022 ao_gps_data.hour = nav_timeutc.hour;
1023 ao_gps_data.minute = nav_timeutc.min;
1024 ao_gps_data.second = nav_timeutc.sec;
1026 /* ublox speeds are cm/s */
1027 ao_gps_data.ground_speed = (uint16_t) nav_velned.g_speed;
1028 ao_gps_data.climb_rate = -(int16_t) nav_velned.vel_d;
1029 ao_gps_data.course = (uint8_t) (nav_velned.heading / 200000);
1032 ao_gps_tracking_data.channels = 0;
1034 struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
1035 #if AO_UBLOX_VERSION >= 10
1036 struct nav_sat_sat *src = &nav_sat_sat[0];
1038 for (i = 0; i < nav_sat_nsat; i++) {
1039 if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
1040 dst->svid = src->svid;
1041 dst->c_n_1 = src->cno;
1043 ao_gps_tracking_data.channels++;
1048 struct nav_svinfo_sat *src = &nav_svinfo_sat[0];
1050 for (i = 0; i < nav_svinfo_nsat; i++) {
1051 if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
1052 src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
1054 if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
1055 dst->svid = src->svid;
1056 dst->c_n_1 = src->cno;
1058 ao_gps_tracking_data.channels++;
1064 ao_mutex_put(&ao_gps_mutex);
1065 ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
1066 ao_wakeup(&ao_gps_new);
1071 static void ao_gps_option(void)
1073 uint8_t r = (uint8_t) ao_cmd_hex();
1074 if (ao_cmd_status != ao_cmd_success) {
1075 ao_cmd_status = ao_cmd_success;
1078 ao_gps_dbg_enable = r;
1079 printf ("gps debug set to %d\n", ao_gps_dbg_enable);
1083 #define ao_gps_option ao_gps_show
1086 const struct ao_cmds ao_gps_cmds[] = {
1087 { ao_gps_option, "g\0Display GPS" },
1091 struct ao_task ao_gps_task;
1096 ao_cmd_register(&ao_gps_cmds[0]);
1097 ao_add_task(&ao_gps_task, ao_gps, "gps");