altos: Add debugging to ublox GPS driver

The new max 7 parts seem to be unhappy about switching baud rates, so
I've added a pile of debugging to help out. Some day, I'll figure out
how to make them work, this code is being left in place to help with that.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index fa6ff0e0724b8290a483ee651bc6583c623851fc..a11ca3f731657f7351b3759ac85bdd09d7a4f32b 100644 (file)
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -21,6 +21,10 @@
 
 #include "ao_gps_ublox.h"
 
+#define AO_UBLOX_DEBUG 1
+
+#include <stdarg.h>
+
 __xdata uint8_t ao_gps_mutex;
 __pdata uint16_t ao_gps_tick;
 __xdata struct ao_telemetry_location   ao_gps_data;
@@ -49,7 +53,34 @@ struct ao_ublox_cksum {
 static __pdata struct ao_ublox_cksum ao_ublox_cksum;
 static __pdata uint16_t ao_ublox_len;
 
-#define ao_ublox_byte()        ((uint8_t) ao_gps_getchar())
+#if AO_UBLOX_DEBUG
+
+static uint8_t ao_gps_dbg_enable;
+
+#define DBG_PROTO      1
+#define DBG_CHAR       2
+#define DBG_INIT       4
+
+static void ao_gps_dbg(int level, char *format, ...) {
+       va_list a;
+
+       if (level & ao_gps_dbg_enable) {
+               va_start(a, format);
+               vprintf(format, a);
+               va_end(a);
+               flush();
+       }
+}
+
+#else
+#define ao_gps_dbg(fmt, ...)
+#endif
+
+static inline uint8_t ao_ublox_byte(void) {
+       uint8_t c = (uint8_t) ao_gps_getchar();
+       ao_gps_dbg(DBG_CHAR, " %02x", c);
+       return c;
+}
 
 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
 {
@@ -65,6 +96,7 @@ static void ao_ublox_init_cksum(void)
 static void ao_ublox_put_u8(uint8_t c)
 {
        add_cksum(&ao_ublox_cksum, c);
+       ao_gps_dbg(DBG_CHAR, " (%02x)", c);
        ao_gps_putchar(c);
 }
 
@@ -187,6 +219,7 @@ ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __r
                        break;
                }
        }
+       ao_gps_dbg(DBG_PROTO, "\n");
 }
 
 /*
@@ -330,6 +363,7 @@ ao_ublox_parse_nav_svinfo(void)
 {
        uint8_t nsat;
        nav_svinfo_nsat = 0;
+
        ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
        for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
                if (nsat < NAV_SVINFO_MAX_SAT) {
@@ -338,6 +372,17 @@ ao_ublox_parse_nav_svinfo(void)
                        ublox_discard(12);
                }
        }
+#if AO_UBLOX_DEBUG
+       ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
+       for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
+               ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
+                           nsat,
+                           nav_svinfo_sat[nsat].chn,
+                           nav_svinfo_sat[nsat].svid,
+                           nav_svinfo_sat[nsat].flags,
+                           nav_svinfo_sat[nsat].quality,
+                           nav_svinfo_sat[nsat].cno);
+#endif
 }
 
 /*
@@ -406,42 +451,52 @@ ao_ublox_parse_nav_velned(void)
  */
 
 static void
-ao_gps_setup(void)
+ao_gps_delay(void)
 {
-       uint8_t i, k;
-
-       ao_delay(AO_SEC_TO_TICKS(3));
-
-       ao_gps_set_speed(AO_SERIAL_SPEED_9600);
+       uint8_t i;
 
        /*
         * A bunch of nulls so the start bit
         * is clear
         */
+
        for (i = 0; i < 64; i++)
                ao_gps_putchar(0x00);
+}
+
+static void
+ao_gps_setup(void)
+{
+       uint8_t i, k;
+
+       ao_delay(AO_SEC_TO_TICKS(3));
+
+       ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
+       ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 
        /*
         * Send the baud-rate setting and protocol-setting
         * command three times
         */
-       for (k = 0; k < 3; k++)
+       for (k = 0; k < 3; k++) {
+               ao_gps_delay();
+
+               ao_gps_dbg(DBG_INIT, "Send initial setting\n");
                for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
                        ao_gps_putchar(ao_gps_set_nmea[i]);
+       }
+
+       ao_gps_delay();
 
 #if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
+       ao_gps_dbg(DBG_INIT, "Set speed high\n");
        /*
         * Increase the baud rate
         */
        ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
 #endif
 
-       /*
-        * Pad with nulls to give the chip
-        * time to see the baud rate switch
-        */
-       for (i = 0; i < 64; i++)
-               ao_gps_putchar(0x00);
+       ao_gps_delay();
 }
 
 static void
@@ -556,7 +611,7 @@ ao_gps(void) __reentrant
        /* Enable all of the messages we want */
        for (i = 0; i < sizeof (ublox_enable_nav); i++)
                ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
-       
+
        ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
                                         UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
                                         UBLOX_CFG_NAV5_FIXMODE_3D,
@@ -587,6 +642,8 @@ ao_gps(void) __reentrant
                ao_ublox_len = header_byte();
                ao_ublox_len |= header_byte() << 8;
 
+               ao_gps_dbg(DBG_PROTO, "class %02x id %02x len %d\n", class, id, ao_ublox_len);
+
                if (ao_ublox_len > 1023)
                        continue;
 
@@ -627,8 +684,10 @@ ao_gps(void) __reentrant
                        break;
                }
 
-               if (ao_ublox_len != 0)
+               if (ao_ublox_len != 0) {
+                       ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
                        continue;
+               }
 
                /* verify checksum and end sequence */
                cksum.a = ao_ublox_byte();
@@ -654,7 +713,7 @@ ao_gps(void) __reentrant
                                }
                                if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
                                        ao_gps_data.flags |= AO_GPS_DATE_VALID;
-                               
+
                                ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
                                ao_gps_data.latitude = nav_posllh.lat;
                                ao_gps_data.longitude = nav_posllh.lon;
@@ -676,7 +735,7 @@ ao_gps(void) __reentrant
                                ao_gps_data.ground_speed = nav_velned.g_speed;
                                ao_gps_data.climb_rate = -nav_velned.vel_d;
                                ao_gps_data.course = nav_velned.heading / 200000;
-                               
+
                                ao_gps_tracking_data.channels = 0;
 
                                struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
@@ -704,8 +763,24 @@ ao_gps(void) __reentrant
        }
 }
 
+#if AO_UBLOX_DEBUG
+static void ao_gps_option(void)
+{
+       ao_cmd_hex();
+       if (ao_cmd_status != ao_cmd_success) {
+               ao_cmd_status = ao_cmd_success;
+               ao_gps_show();
+       } else {
+               ao_gps_dbg_enable = ao_cmd_lex_i;
+               printf ("gps debug set to %d\n", ao_gps_dbg_enable);
+       }
+}
+#else
+#define ao_gps_option ao_gps_show
+#endif
+
 __code struct ao_cmds ao_gps_cmds[] = {
-       { ao_gps_show,  "g\0Display GPS" },
+       { ao_gps_option,        "g\0Display GPS" },
        { 0, NULL },
 };