altos: Set u-blox navigation settings

Airborne mode, < 4g (as good as it gets)
Only use 3D fixes (2D isn't very useful)

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 574b6a2c5686d0313f6e05b9d3d59d264dea8ea7..22300df3100f636753aa90d4de39519bb0474f21 100644 (file)
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -58,12 +58,43 @@ static void ao_ublox_init_cksum(void)
        ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
 }
 
-static void ao_ublox_putchar_cksum(uint8_t c)
+static void ao_ublox_put_u8(uint8_t c)
 {
        add_cksum(&ao_ublox_cksum, c);
        ao_ublox_putchar(c);
 }
 
+static void ao_ublox_put_i8(int8_t c)
+{
+       ao_ublox_put_u8((uint8_t) c);
+}
+
+static void ao_ublox_put_u16(uint16_t c)
+{
+       ao_ublox_put_u8(c);
+       ao_ublox_put_u8(c>>8);
+}
+
+#if 0
+static void ao_ublox_put_i16(int16_t c)
+{
+       ao_ublox_put_u16((uint16_t) c);
+}
+#endif
+
+static void ao_ublox_put_u32(uint32_t c)
+{
+       ao_ublox_put_u8(c);
+       ao_ublox_put_u8(c>>8);
+       ao_ublox_put_u8(c>>16);
+       ao_ublox_put_u8(c>>24);
+}
+
+static void ao_ublox_put_i32(int32_t c)
+{
+       ao_ublox_put_u32((uint32_t) c);
+}
+
 static uint8_t header_byte(void)
 {
        uint8_t c = ao_ublox_byte();
@@ -404,35 +435,71 @@ ao_gps_setup(void)
                ao_ublox_putchar(0x00);
 }
 
-void
+static void
 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 {
        ao_ublox_init_cksum();
        ao_ublox_putchar(0xb5);
        ao_ublox_putchar(0x62);
-       ao_ublox_putchar_cksum(class);
-       ao_ublox_putchar_cksum(id);
-       ao_ublox_putchar_cksum(len);
-       ao_ublox_putchar_cksum(len >> 8);
+       ao_ublox_put_u8(class);
+       ao_ublox_put_u8(id);
+       ao_ublox_put_u8(len);
+       ao_ublox_put_u8(len >> 8);
 }
 
-void
+static void
 ao_ublox_putend(void)
 {
        ao_ublox_putchar(ao_ublox_cksum.a);
        ao_ublox_putchar(ao_ublox_cksum.b);
 }
 
-void
+static void
 ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
 {
        ao_ublox_putstart(0x06, 0x01, 3);
-       ao_ublox_putchar_cksum(class);
-       ao_ublox_putchar_cksum(msgid);
-       ao_ublox_putchar_cksum(rate);
+       ao_ublox_put_u8(class);
+       ao_ublox_put_u8(msgid);
+       ao_ublox_put_u8(rate);
        ao_ublox_putend();
 }
 
+static void
+ao_ublox_set_navigation_settings(uint16_t mask,
+                                uint8_t dyn_model,
+                                uint8_t fix_mode,
+                                int32_t fixed_alt,
+                                uint32_t fixed_alt_var,
+                                int8_t min_elev,
+                                uint8_t dr_limit,
+                                uint16_t pdop,
+                                uint16_t tdop,
+                                uint16_t pacc,
+                                uint16_t tacc,
+                                uint8_t static_hold_thresh,
+                                uint8_t dgps_time_out)
+{
+       ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
+       ao_ublox_put_u16(mask);
+       ao_ublox_put_u8(dyn_model);
+       ao_ublox_put_u8(fix_mode);
+       ao_ublox_put_i32(fixed_alt);
+       ao_ublox_put_u32(fixed_alt_var);
+       ao_ublox_put_i8(min_elev);
+       ao_ublox_put_u8(dr_limit);
+       ao_ublox_put_u16(pdop);
+       ao_ublox_put_u16(tdop);
+       ao_ublox_put_u16(pacc);
+       ao_ublox_put_u16(tacc);
+       ao_ublox_put_u8(static_hold_thresh);
+       ao_ublox_put_u8(dgps_time_out);
+       ao_ublox_put_u32(0);
+       ao_ublox_put_u32(0);
+       ao_ublox_put_u32(0);
+       ao_ublox_putend();
+}
+
+
 /*
  * Disable all MON message
  */
@@ -481,6 +548,20 @@ ao_gps(void) __reentrant
        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,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0,
+                                        0);
+
        for (;;) {
                /* Locate the begining of the next record */
                while (ao_ublox_byte() != (uint8_t) 0xb5)

diff --git a/src/drivers/ao_gps_ublox.h b/src/drivers/ao_gps_ublox.h
index 562a43543f9722d6cc86d99c85818ab1720c13dc..e4a358a87a23d50852d459f0d52862ce2c3d5318 100644 (file)
--- a/src/drivers/ao_gps_ublox.h
+++ b/src/drivers/ao_gps_ublox.h
@@ -238,4 +238,30 @@ struct ublox_nav_velned {
        uint32_t        cacc;           /* deg */
 };
 
+#define UBLOX_CFG      0x06
+
+#define UBLOX_CFG_NAV5 0x24
+
+#define UBLOX_CFG_NAV5_MASK_DYN                        0
+#define UBLOX_CFG_NAV5_MASK_MINE1              1
+#define UBLOX_CFG_NAV5_MASK_FIXMODE            2
+#define UBLOX_CFG_NAV5_MASK_DRLIM              3
+#define UBLOX_CFG_NAV5_MASK_POSMASK            4
+#define UBLOX_CFG_NAV5_MASK_TIMEMASK           5
+#define UBLOX_CFG_NAV5_MASK_STATICHOLDMASK     6
+#define UBLOX_CFG_NAV5_MASK_DGPSMASK           7
+
+#define UBLOX_CFG_NAV5_DYNMODEL_PORTABLE       0
+#define UBLOX_CFG_NAV5_DYNMODEL_STATIONARY     2
+#define UBLOX_CFG_NAV5_DYNMODEL_PEDESTRIAN     3
+#define UBLOX_CFG_NAV5_DYNMODEL_AUTOMOTIVE     4
+#define UBLOX_CFG_NAV5_DYNMODEL_SEA            5
+#define UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_1G    6
+#define UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_2G    7
+#define UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G    8
+
+#define UBLOX_CFG_NAV5_FIXMODE_2D              1
+#define UBLOX_CFG_NAV5_FIXMODE_3D              2
+#define UBLOX_CFG_NAV5_FIXMODE_AUTO            3
+
 #endif /* _AO_GPS_UBLOX_H_ */