X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=2774619a053e87db599e47a7309b4c9ea8c9bdf6;hb=20523344d3d9e5fb01166a1452e833705c6ce783;hp=32405ea5db502d9d2cfc7e3d559a3a0d8ba3589a;hpb=50457f9983ec0a432f1050464382749436e3da94;p=fw%2Faltos diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 32405ea5..2774619a 100644 --- 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 */ @@ -450,6 +517,18 @@ static const uint8_t ublox_disable_nav[] = { 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 { @@ -459,31 +538,30 @@ 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) @@ -617,10 +695,35 @@ ao_gps(void) __reentrant } } +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"); }