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();
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
*/
0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12
};
+/*
+ * Enable enough messages to get all of the data we want
+ */
+static const uint8_t ublox_enable_nav[] = {
+ UBLOX_NAV_DOP,
+ UBLOX_NAV_POSLLH,
+ UBLOX_NAV_SOL,
+ UBLOX_NAV_SVINFO,
+ UBLOX_NAV_VELNED,
+ UBLOX_NAV_TIMEUTC
+};
+
void
ao_gps(void) __reentrant
{
ao_gps_setup();
+ /* Disable all messages */
for (i = 0; i < sizeof (ublox_disable_mon); i++)
ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0);
for (i = 0; i < sizeof (ublox_disable_nav); i++)
- ao_ublox_set_message_rate(0x01, ublox_disable_nav[i], 0);
+ ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
/* Enable all of the messages we want */
-
- /* DOP */
- ao_ublox_set_message_rate(0x01, 0x04, 1);
-
- /* POSLLH */
- ao_ublox_set_message_rate(0x01, 0x02, 1);
-
- /* SOL */
- ao_ublox_set_message_rate(0x01, 0x06, 1);
-
- /* SVINFO */
- ao_ublox_set_message_rate(0x01, 0x30, 1);
-
- /* VELNED */
- ao_ublox_set_message_rate(0x01, 0x12, 1);
-
- /* TIMEUTC */
- ao_ublox_set_message_rate(0x01, 0x21, 1);
+ 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)
}
}
+static void
+gps_dump(void) __reentrant
+{
+ uint8_t i;
+ ao_mutex_get(&ao_gps_mutex);
+ printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
+ printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
+ printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude);
+ printf ("Alt: %d\n", ao_gps_data.altitude);
+ printf ("Flags: 0x%x\n", ao_gps_data.flags);
+ printf ("Sats: %d", ao_gps_tracking_data.channels);
+ for (i = 0; i < ao_gps_tracking_data.channels; i++)
+ printf (" %d %d",
+ ao_gps_tracking_data.sats[i].svid,
+ ao_gps_tracking_data.sats[i].c_n_1);
+ printf ("\ndone\n");
+ ao_mutex_put(&ao_gps_mutex);
+}
+
+__code struct ao_cmds ao_gps_cmds[] = {
+ { gps_dump, "g\0Display GPS" },
+ { 0, NULL },
+};
+
__xdata struct ao_task ao_gps_task;
void
ao_gps_init(void)
{
+ ao_cmd_register(&ao_gps_cmds[0]);
ao_add_task(&ao_gps_task, ao_gps, "gps");
}