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
31 AO_TICK_TYPE ao_gps_tick;
32 AO_TICK_TYPE ao_gps_utc_tick;
33 struct ao_telemetry_location ao_gps_data;
34 struct ao_telemetry_satellite ao_gps_tracking_data;
36 #undef AO_SERIAL_SPEED_UBLOX
38 #ifndef AO_SERIAL_SPEED_UBLOX
39 #define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600
42 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
43 #define SERIAL_SPEED_STRING "57600"
44 #define SERIAL_SPEED_CHECKSUM "2d"
46 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
47 #define SERIAL_SPEED_STRING "19200"
48 #define SERIAL_SPEED_CHECKSUM "23"
50 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
51 #define SERIAL_SPEED_STRING "9600"
52 #define SERIAL_SPEED_CHECKSUM "16"
55 static const char ao_gps_set_nmea[] =
56 "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
58 struct ao_ublox_cksum {
62 static struct ao_ublox_cksum ao_ublox_cksum;
63 static uint16_t ao_ublox_len;
72 static uint8_t ao_gps_dbg_enable = DBG_PROTO|DBG_CHAR|DBG_INIT;
74 static void ao_gps_dbg(int level, char *format, ...) {
77 if (level & ao_gps_dbg_enable) {
86 #define ao_gps_dbg(fmt, ...)
89 static inline uint8_t ao_ublox_byte(void) {
90 uint8_t c = (uint8_t) ao_gps_getchar();
91 ao_gps_dbg(DBG_CHAR, " %02x", c);
95 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
101 static void ao_ublox_init_cksum(void)
103 ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
106 static void ao_ublox_put_u8(uint8_t c)
108 add_cksum(&ao_ublox_cksum, c);
109 ao_gps_dbg(DBG_CHAR, " (%02x)", c);
113 static void ao_ublox_put_i8(int8_t c)
115 ao_ublox_put_u8((uint8_t) c);
118 static void ao_ublox_put_u16(uint16_t c)
120 ao_ublox_put_u8((uint8_t) c);
121 ao_ublox_put_u8((uint8_t) (c>>8));
125 static void ao_ublox_put_i16(int16_t c)
127 ao_ublox_put_u16((uint16_t) c);
131 static void ao_ublox_put_u32(uint32_t c)
133 ao_ublox_put_u8((uint8_t) c);
134 ao_ublox_put_u8((uint8_t) (c>>8));
135 ao_ublox_put_u8((uint8_t) (c>>16));
136 ao_ublox_put_u8((uint8_t) (c>>24));
139 static void ao_ublox_put_i32(int32_t c)
141 ao_ublox_put_u32((uint32_t) c);
144 static uint8_t header_byte(void)
146 uint8_t c = ao_ublox_byte();
147 add_cksum(&ao_ublox_cksum, c);
151 static uint8_t data_byte(void)
154 return header_byte();
157 static char *ublox_target;
159 static void ublox_u16(uint8_t offset)
161 uint16_t *ptr = (uint16_t *) (void *) (ublox_target + offset);
165 val |= (uint16_t) ((uint16_t) data_byte () << 8);
169 static void ublox_u8(uint8_t offset)
171 uint8_t *ptr = (uint8_t *) (ublox_target + offset);
178 static void ublox_u32(uint8_t offset)
180 uint32_t *ptr = (uint32_t *) (void *) (ublox_target + offset);
183 val = ((uint32_t) data_byte ());
184 val |= ((uint32_t) data_byte ()) << 8;
185 val |= ((uint32_t) data_byte ()) << 16;
186 val |= ((uint32_t) data_byte ()) << 24;
190 static void ublox_discard(uint8_t len)
197 #define UBLOX_DISCARD 1
202 struct ublox_packet_parse {
208 ao_ublox_parse(void *target, const struct ublox_packet_parse *parse)
212 ublox_target = target;
214 offset = parse[i].offset;
215 switch (parse[i].type) {
219 ublox_discard(offset);
232 ao_gps_dbg(DBG_PROTO, "\n");
236 * NAV-DOP message parsing
239 static struct nav_dop {
245 static const struct ublox_packet_parse nav_dop_packet[] = {
246 { UBLOX_DISCARD, 6 }, /* 0 GPS Millisecond Time of Week, gDOP */
247 { UBLOX_U16, offsetof(struct nav_dop, pdop) }, /* 6 pDOP */
248 { UBLOX_DISCARD, 2 }, /* 8 tDOP */
249 { UBLOX_U16, offsetof(struct nav_dop, vdop) }, /* 10 vDOP */
250 { UBLOX_U16, offsetof(struct nav_dop, hdop) }, /* 12 hDOP */
251 { UBLOX_DISCARD, 4 }, /* 14 nDOP, eDOP */
256 ao_ublox_parse_nav_dop(void)
258 ao_ublox_parse(&nav_dop, nav_dop_packet);
262 * NAV-POSLLH message parsing
264 static struct nav_posllh {
270 static const struct ublox_packet_parse nav_posllh_packet[] = {
271 { UBLOX_DISCARD, 4 }, /* 0 GPS Millisecond Time of Week */
272 { UBLOX_U32, offsetof(struct nav_posllh, lon) }, /* 4 Longitude */
273 { UBLOX_U32, offsetof(struct nav_posllh, lat) }, /* 8 Latitude */
274 { UBLOX_DISCARD, 4 }, /* 12 Height above Ellipsoid */
275 { UBLOX_U32, offsetof(struct nav_posllh, alt_msl) }, /* 16 Height above mean sea level */
276 { UBLOX_DISCARD, 8 }, /* 20 hAcc, vAcc */
277 { UBLOX_END, 0 }, /* 28 */
281 ao_ublox_parse_nav_posllh(void)
283 ao_ublox_parse(&nav_posllh, nav_posllh_packet);
287 * NAV-SOL message parsing
289 static struct nav_sol {
295 static const struct ublox_packet_parse nav_sol_packet[] = {
296 { UBLOX_DISCARD, 10 }, /* 0 iTOW, fTOW, week */
297 { UBLOX_U8, offsetof(struct nav_sol, gps_fix) }, /* 10 gpsFix */
298 { UBLOX_U8, offsetof(struct nav_sol, flags) }, /* 11 flags */
299 { UBLOX_DISCARD, 35 }, /* 12 ecefX, ecefY, ecefZ, pAcc, ecefVX, ecefVY, ecefVZ, sAcc, pDOP, reserved1 */
300 { UBLOX_U8, offsetof(struct nav_sol, nsat) }, /* 47 numSV */
301 { UBLOX_DISCARD, 4 }, /* 48 reserved2 */
305 #define NAV_SOL_FLAGS_GPSFIXOK 0
306 #define NAV_SOL_FLAGS_DIFFSOLN 1
307 #define NAV_SOL_FLAGS_WKNSET 2
308 #define NAV_SOL_FLAGS_TOWSET 3
311 ao_ublox_parse_nav_sol(void)
313 ao_ublox_parse(&nav_sol, nav_sol_packet);
317 * NAV-SVINFO message parsing
320 static struct nav_svinfo {
325 static const struct ublox_packet_parse nav_svinfo_packet[] = {
326 { UBLOX_DISCARD, 4 }, /* 0 iTOW */
327 { UBLOX_U8, offsetof(struct nav_svinfo, num_ch) }, /* 4 numCh */
328 { UBLOX_U8, offsetof(struct nav_svinfo, flags) }, /* 5 globalFlags */
329 { UBLOX_DISCARD, 2 }, /* 6 reserved2 */
333 #define NAV_SVINFO_MAX_SAT 16
335 static struct nav_svinfo_sat {
341 } nav_svinfo_sat[NAV_SVINFO_MAX_SAT];
343 static uint8_t nav_svinfo_nsat;
345 static const struct ublox_packet_parse nav_svinfo_sat_packet[] = {
346 { UBLOX_U8, offsetof(struct nav_svinfo_sat, chn) }, /* 8 + 12*N chn */
347 { UBLOX_U8, offsetof(struct nav_svinfo_sat, svid) }, /* 9 + 12*N svid */
348 { UBLOX_U8, offsetof(struct nav_svinfo_sat, flags) }, /* 10 + 12*N flags */
349 { UBLOX_U8, offsetof(struct nav_svinfo_sat, quality) }, /* 11 + 12*N quality */
350 { UBLOX_U8, offsetof(struct nav_svinfo_sat, cno) }, /* 12 + 12*N cno */
351 { UBLOX_DISCARD, 7 }, /* 13 + 12*N elev, azim, prRes */
355 #define NAV_SVINFO_SAT_FLAGS_SVUSED 0
356 #define NAV_SVINFO_SAT_FLAGS_DIFFCORR 1
357 #define NAV_SVINFO_SAT_FLAGS_ORBITAVAIL 2
358 #define NAV_SVINFO_SAT_FLAGS_ORBITEPH 3
359 #define NAV_SVINFO_SAT_FLAGS_UNHEALTHY 4
360 #define NAV_SVINFO_SAT_FLAGS_ORBITALM 5
361 #define NAV_SVINFO_SAT_FLAGS_ORBITAOP 6
362 #define NAV_SVINFO_SAT_FLAGS_SMOOTHED 7
364 #define NAV_SVINFO_SAT_QUALITY_IDLE 0
365 #define NAV_SVINFO_SAT_QUALITY_SEARCHING 1
366 #define NAV_SVINFO_SAT_QUALITY_ACQUIRED 2
367 #define NAV_SVINFO_SAT_QUALITY_UNUSABLE 3
368 #define NAV_SVINFO_SAT_QUALITY_LOCKED 4
369 #define NAV_SVINFO_SAT_QUALITY_RUNNING 5
372 ao_ublox_parse_nav_svinfo(void)
377 ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
378 for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
379 if (nsat < NAV_SVINFO_MAX_SAT) {
380 ao_ublox_parse(&nav_svinfo_sat[nav_svinfo_nsat++], nav_svinfo_sat_packet);
386 ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
387 for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
388 ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
390 nav_svinfo_sat[nsat].chn,
391 nav_svinfo_sat[nsat].svid,
392 nav_svinfo_sat[nsat].flags,
393 nav_svinfo_sat[nsat].quality,
394 nav_svinfo_sat[nsat].cno);
399 * NAV-TIMEUTC message parsing
401 static struct nav_timeutc {
412 #define NAV_TIMEUTC_VALID_TOW 0
413 #define NAV_TIMEUTC_VALID_WKN 1
414 #define NAV_TIMEUTC_VALID_UTC 2
416 static const struct ublox_packet_parse nav_timeutc_packet[] = {
417 { UBLOX_DISCARD, 8 }, /* 0 iTOW, tAcc */
418 { UBLOX_U32, offsetof(struct nav_timeutc, nano) }, /* 8 nano */
419 { UBLOX_U16, offsetof(struct nav_timeutc, year) }, /* 12 year */
420 { UBLOX_U8, offsetof(struct nav_timeutc, month) }, /* 14 month */
421 { UBLOX_U8, offsetof(struct nav_timeutc, day) }, /* 15 day */
422 { UBLOX_U8, offsetof(struct nav_timeutc, hour) }, /* 16 hour */
423 { UBLOX_U8, offsetof(struct nav_timeutc, min) }, /* 17 min */
424 { UBLOX_U8, offsetof(struct nav_timeutc, sec) }, /* 18 sec */
425 { UBLOX_U8, offsetof(struct nav_timeutc, valid) }, /* 19 valid */
430 ao_ublox_parse_nav_timeutc(void)
432 ao_ublox_parse(&nav_timeutc, nav_timeutc_packet);
436 * NAV-VELNED message parsing
439 static struct nav_velned {
445 static const struct ublox_packet_parse nav_velned_packet[] = {
446 { UBLOX_DISCARD, 12 }, /* 0 iTOW, velN, velE */
447 { UBLOX_U32, offsetof(struct nav_velned, vel_d) }, /* 12 velD */
448 { UBLOX_DISCARD, 4 }, /* 16 speed */
449 { UBLOX_U32, offsetof(struct nav_velned, g_speed) }, /* 20 gSpeed */
450 { UBLOX_U32, offsetof(struct nav_velned, heading) }, /* 24 heading */
451 { UBLOX_DISCARD, 8 }, /* 28 sAcc, cAcc */
456 ao_ublox_parse_nav_velned(void)
458 ao_ublox_parse(&nav_velned, nav_velned_packet);
462 * Set the protocol mode and baud rate
471 * A bunch of nulls so the start bit
475 for (i = 0; i < 64; i++)
476 ao_gps_putchar(0x00);
484 ao_delay(AO_SEC_TO_TICKS(3));
486 ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
487 ao_gps_set_speed(AO_SERIAL_SPEED_9600);
490 * Send the baud-rate setting and protocol-setting
491 * command three times
493 for (k = 0; k < 3; k++) {
496 ao_gps_dbg(DBG_INIT, "Send initial setting\n");
497 for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
498 ao_gps_putchar(ao_gps_set_nmea[i]);
503 #if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
504 ao_gps_dbg(DBG_INIT, "Set speed high\n");
506 * Increase the baud rate
508 ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
515 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
517 ao_ublox_init_cksum();
518 ao_gps_putchar(0xb5);
519 ao_gps_putchar(0x62);
520 ao_ublox_put_u8(class);
522 ao_ublox_put_u8((uint8_t) len);
523 ao_ublox_put_u8((uint8_t) (len >> 8));
527 ao_ublox_putend(void)
529 ao_gps_putchar(ao_ublox_cksum.a);
530 ao_gps_putchar(ao_ublox_cksum.b);
534 ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
536 ao_ublox_putstart(0x06, 0x01, 3);
537 ao_ublox_put_u8(class);
538 ao_ublox_put_u8(msgid);
539 ao_ublox_put_u8(rate);
544 ao_ublox_set_navigation_settings(uint16_t mask,
548 uint32_t fixed_alt_var,
555 uint8_t static_hold_thresh,
556 uint8_t dgps_time_out)
558 ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
559 ao_ublox_put_u16(mask);
560 ao_ublox_put_u8(dyn_model);
561 ao_ublox_put_u8(fix_mode);
562 ao_ublox_put_i32(fixed_alt);
563 ao_ublox_put_u32(fixed_alt_var);
564 ao_ublox_put_i8(min_elev);
565 ao_ublox_put_u8(dr_limit);
566 ao_ublox_put_u16(pdop);
567 ao_ublox_put_u16(tdop);
568 ao_ublox_put_u16(pacc);
569 ao_ublox_put_u16(tacc);
570 ao_ublox_put_u8(static_hold_thresh);
571 ao_ublox_put_u8(dgps_time_out);
580 * Disable all MON message
582 static const uint8_t ublox_disable_mon[] = {
583 0x0b, 0x09, 0x02, 0x06, 0x07, 0x21, 0x08, 0x04
587 * Disable all NAV messages. The desired
588 * ones will be explicitly re-enabled
591 static const uint8_t ublox_disable_nav[] = {
592 0x60, 0x22, 0x31, 0x04, 0x40, 0x01, 0x02, 0x32,
593 0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12
597 * Enable enough messages to get all of the data we want
599 static const uint8_t ublox_enable_nav[] = {
609 ao_gps_set_rate(uint8_t rate)
612 for (i = 0; i < sizeof (ublox_enable_nav); i++)
613 ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate);
620 struct ao_ublox_cksum cksum;
622 AO_TICK_TYPE packet_start_tick;
623 AO_TICK_TYPE solution_tick = 0;
627 /* Disable all messages */
628 for (i = 0; i < sizeof (ublox_disable_mon); i++)
629 ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0);
630 for (i = 0; i < sizeof (ublox_disable_nav); i++)
631 ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
633 /* Enable all of the messages we want */
636 ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
637 UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
638 UBLOX_CFG_NAV5_FIXMODE_3D,
651 /* Locate the begining of the next record */
652 while (ao_ublox_byte() != (uint8_t) 0xb5)
654 packet_start_tick = ao_tick_count;
656 if (ao_ublox_byte() != (uint8_t) 0x62)
659 ao_ublox_init_cksum();
661 class = header_byte();
665 ao_ublox_len = header_byte();
666 ao_ublox_len |= (uint16_t) ((uint16_t) header_byte() << 8);
668 ao_gps_dbg(DBG_PROTO, "%6u class %02x id %02x len %d\n", packet_start_tick, class, id, ao_ublox_len);
670 if (ao_ublox_len > 1023)
677 if (ao_ublox_len != 18)
679 ao_ublox_parse_nav_dop();
681 case UBLOX_NAV_POSLLH:
682 if (ao_ublox_len != 28)
684 ao_ublox_parse_nav_posllh();
687 if (ao_ublox_len != 52)
689 ao_ublox_parse_nav_sol();
690 solution_tick = packet_start_tick;
692 case UBLOX_NAV_SVINFO:
693 if (ao_ublox_len < 8)
695 ao_ublox_parse_nav_svinfo();
697 case UBLOX_NAV_VELNED:
698 if (ao_ublox_len != 36)
700 ao_ublox_parse_nav_velned();
702 case UBLOX_NAV_TIMEUTC:
703 if (ao_ublox_len != 20)
705 ao_ublox_parse_nav_timeutc();
711 if (ao_ublox_len != 0) {
712 ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
716 /* verify checksum and end sequence */
717 cksum.a = ao_ublox_byte();
718 cksum.b = ao_ublox_byte();
719 if (ao_ublox_cksum.a != cksum.a || ao_ublox_cksum.b != cksum.b)
725 case UBLOX_NAV_TIMEUTC:
726 ao_mutex_get(&ao_gps_mutex);
727 ao_gps_tick = solution_tick;
728 ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_timeutc.nano);
729 ao_gps_data.flags = 0;
730 ao_gps_data.flags |= AO_GPS_RUNNING;
731 if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
732 uint8_t nsat = nav_sol.nsat;
733 ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
736 ao_gps_data.flags |= nsat;
738 if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
739 ao_gps_data.flags |= AO_GPS_DATE_VALID;
741 AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000);
742 ao_gps_data.latitude = nav_posllh.lat;
743 ao_gps_data.longitude = nav_posllh.lon;
745 ao_gps_data.year = (uint8_t) (nav_timeutc.year - 2000);
746 ao_gps_data.month = nav_timeutc.month;
747 ao_gps_data.day = nav_timeutc.day;
749 ao_gps_data.hour = nav_timeutc.hour;
750 ao_gps_data.minute = nav_timeutc.min;
751 ao_gps_data.second = nav_timeutc.sec;
753 /* we report dop scaled by 10, but ublox provides dop scaled by 100
755 ao_gps_data.pdop = (uint8_t) (nav_dop.pdop / 10);
756 ao_gps_data.hdop = (uint8_t) (nav_dop.hdop / 10);
757 ao_gps_data.vdop = (uint8_t) (nav_dop.vdop / 10);
759 ao_gps_data.ground_speed = (uint16_t) nav_velned.g_speed;
760 ao_gps_data.climb_rate = -(int16_t) nav_velned.vel_d;
761 ao_gps_data.course = (uint8_t) (nav_velned.heading / 200000);
763 ao_gps_tracking_data.channels = 0;
765 struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
766 struct nav_svinfo_sat *src = &nav_svinfo_sat[0];
768 for (i = 0; i < nav_svinfo_nsat; i++) {
769 if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
770 src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
772 if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
773 dst->svid = src->svid;
774 dst->c_n_1 = src->cno;
776 ao_gps_tracking_data.channels++;
782 ao_mutex_put(&ao_gps_mutex);
783 ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
784 ao_wakeup(&ao_gps_new);
793 static void ao_gps_option(void)
795 uint8_t r = (uint8_t) ao_cmd_hex();
796 if (ao_cmd_status != ao_cmd_success) {
797 ao_cmd_status = ao_cmd_success;
800 ao_gps_dbg_enable = r;
801 printf ("gps debug set to %d\n", ao_gps_dbg_enable);
805 #define ao_gps_option ao_gps_show
808 const struct ao_cmds ao_gps_cmds[] = {
809 { ao_gps_option, "g\0Display GPS" },
813 struct ao_task ao_gps_task;
818 ao_cmd_register(&ao_gps_cmds[0]);
819 ao_add_task(&ao_gps_task, ao_gps, "gps");