X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=fa6ff0e0724b8290a483ee651bc6583c623851fc;hp=32405ea5db502d9d2cfc7e3d559a3a0d8ba3589a;hb=9a22a300009679a14d66214a5d61e9e6a177279f;hpb=50457f9983ec0a432f1050464382749436e3da94 diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 32405ea5..fa6ff0e0 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -26,11 +26,21 @@ __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; __xdata struct ao_telemetry_satellite ao_gps_tracking_data; -static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n"; +#ifndef AO_SERIAL_SPEED_UBLOX +#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_57600 +#endif -const char ao_gps_config[] = { +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600 +#define SERIAL_SPEED_STRING "57600" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200 +#define SERIAL_SPEED_STRING "19200" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600 +#define SERIAL_SPEED_STRING "9600" +#endif -}; +static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*2d\r\n"; struct ao_ublox_cksum { uint8_t a, b; @@ -39,13 +49,7 @@ struct ao_ublox_cksum { static __pdata struct ao_ublox_cksum ao_ublox_cksum; static __pdata uint16_t ao_ublox_len; -#ifndef ao_ublox_getchar -#define ao_ublox_getchar ao_serial1_getchar -#define ao_ublox_putchar ao_serial1_putchar -#define ao_ublox_set_speed ao_serial1_set_speed -#endif - -#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar()) +#define ao_ublox_byte() ((uint8_t) ao_gps_getchar()) static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c) { @@ -58,10 +62,41 @@ 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); + ao_gps_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) @@ -374,14 +409,17 @@ static void ao_gps_setup(void) { uint8_t i, k; - ao_ublox_set_speed(AO_SERIAL_SPEED_9600); + + ao_delay(AO_SEC_TO_TICKS(3)); + + ao_gps_set_speed(AO_SERIAL_SPEED_9600); /* * A bunch of nulls so the start bit * is clear */ for (i = 0; i < 64; i++) - ao_ublox_putchar(0x00); + ao_gps_putchar(0x00); /* * Send the baud-rate setting and protocol-setting @@ -389,50 +427,88 @@ ao_gps_setup(void) */ for (k = 0; k < 3; k++) for (i = 0; i < sizeof (ao_gps_set_nmea); i++) - ao_ublox_putchar(ao_gps_set_nmea[i]); + ao_gps_putchar(ao_gps_set_nmea[i]); +#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600 /* * Increase the baud rate */ - ao_ublox_set_speed(AO_SERIAL_SPEED_57600); + 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_ublox_putchar(0x00); + ao_gps_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_gps_putchar(0xb5); + ao_gps_putchar(0x62); + 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); + ao_gps_putchar(ao_ublox_cksum.a); + ao_gps_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 +526,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 +547,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 +704,16 @@ ao_gps(void) __reentrant } } +__code struct ao_cmds ao_gps_cmds[] = { + { ao_gps_show, "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"); }