X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=3d615e9c933f14b260f3cd0fb75faeec68073a3c;hb=560767497057c08a6fc3bc24aaba23a8d6192259;hp=a6d930835baed49a70d619073b84349242b0a806;hpb=0686a7b8aec524d81bda4c572549a3a068ce0eed;p=fw%2Faltos diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index a6d93083..3d615e9c 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -28,7 +28,8 @@ uint8_t ao_gps_new; uint8_t ao_gps_mutex; -uint16_t ao_gps_tick; +AO_TICK_TYPE ao_gps_tick; +AO_TICK_TYPE ao_gps_utc_tick; struct ao_telemetry_location ao_gps_data; struct ao_telemetry_satellite ao_gps_tracking_data; @@ -63,12 +64,13 @@ static uint16_t ao_ublox_len; #if AO_UBLOX_DEBUG -static uint8_t ao_gps_dbg_enable; #define DBG_PROTO 1 #define DBG_CHAR 2 #define DBG_INIT 4 +static uint8_t ao_gps_dbg_enable = DBG_PROTO|DBG_CHAR|DBG_INIT; + static void ao_gps_dbg(int level, char *format, ...) { va_list a; @@ -115,8 +117,8 @@ static void ao_ublox_put_i8(int8_t c) static void ao_ublox_put_u16(uint16_t c) { - ao_ublox_put_u8(c); - ao_ublox_put_u8(c>>8); + ao_ublox_put_u8((uint8_t) c); + ao_ublox_put_u8((uint8_t) (c>>8)); } #if 0 @@ -128,10 +130,10 @@ static void ao_ublox_put_i16(int16_t c) 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); + ao_ublox_put_u8((uint8_t) c); + ao_ublox_put_u8((uint8_t) (c>>8)); + ao_ublox_put_u8((uint8_t) (c>>16)); + ao_ublox_put_u8((uint8_t) (c>>24)); } static void ao_ublox_put_i32(int32_t c) @@ -160,7 +162,7 @@ static void ublox_u16(uint8_t offset) uint16_t val; val = data_byte(); - val |= data_byte () << 8; + val |= (uint16_t) ((uint16_t) data_byte () << 8); *ptr = val; } @@ -397,6 +399,7 @@ ao_ublox_parse_nav_svinfo(void) * NAV-TIMEUTC message parsing */ static struct nav_timeutc { + int32_t nano; uint16_t year; uint8_t month; uint8_t day; @@ -411,7 +414,8 @@ static struct nav_timeutc { #define NAV_TIMEUTC_VALID_UTC 2 static const struct ublox_packet_parse nav_timeutc_packet[] = { - { UBLOX_DISCARD, 12 }, /* 0 iTOW, tAcc, nano */ + { UBLOX_DISCARD, 8 }, /* 0 iTOW, tAcc */ + { UBLOX_U32, offsetof(struct nav_timeutc, nano) }, /* 8 nano */ { UBLOX_U16, offsetof(struct nav_timeutc, year) }, /* 12 year */ { UBLOX_U8, offsetof(struct nav_timeutc, month) }, /* 14 month */ { UBLOX_U8, offsetof(struct nav_timeutc, day) }, /* 15 day */ @@ -515,8 +519,8 @@ ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len) 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); + ao_ublox_put_u8((uint8_t) len); + ao_ublox_put_u8((uint8_t) (len >> 8)); } static void @@ -615,6 +619,8 @@ ao_gps(void) uint8_t class, id; struct ao_ublox_cksum cksum; uint8_t i; + AO_TICK_TYPE packet_start_tick; + AO_TICK_TYPE solution_tick = 0; ao_gps_setup(); @@ -645,6 +651,8 @@ ao_gps(void) /* Locate the begining of the next record */ while (ao_ublox_byte() != (uint8_t) 0xb5) ; + packet_start_tick = ao_tick_count; + if (ao_ublox_byte() != (uint8_t) 0x62) continue; @@ -655,9 +663,9 @@ ao_gps(void) /* Length */ ao_ublox_len = header_byte(); - ao_ublox_len |= header_byte() << 8; + ao_ublox_len |= (uint16_t) ((uint16_t) header_byte() << 8); - ao_gps_dbg(DBG_PROTO, "class %02x id %02x len %d\n", class, id, ao_ublox_len); + ao_gps_dbg(DBG_PROTO, "%6u class %02x id %02x len %d\n", packet_start_tick, class, id, ao_ublox_len); if (ao_ublox_len > 1023) continue; @@ -679,6 +687,7 @@ ao_gps(void) if (ao_ublox_len != 52) break; ao_ublox_parse_nav_sol(); + solution_tick = packet_start_tick; break; case UBLOX_NAV_SVINFO: if (ao_ublox_len < 8) @@ -715,8 +724,8 @@ ao_gps(void) switch (id) { case UBLOX_NAV_TIMEUTC: ao_mutex_get(&ao_gps_mutex); - ao_gps_tick = ao_time(); - + ao_gps_tick = solution_tick; + ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_timeutc.nano); ao_gps_data.flags = 0; ao_gps_data.flags |= AO_GPS_RUNNING; if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) { @@ -733,7 +742,7 @@ ao_gps(void) ao_gps_data.latitude = nav_posllh.lat; ao_gps_data.longitude = nav_posllh.lon; - ao_gps_data.year = nav_timeutc.year - 2000; + ao_gps_data.year = (uint8_t) (nav_timeutc.year - 2000); ao_gps_data.month = nav_timeutc.month; ao_gps_data.day = nav_timeutc.day; @@ -743,13 +752,13 @@ ao_gps(void) /* we report dop scaled by 10, but ublox provides dop scaled by 100 */ - ao_gps_data.pdop = nav_dop.pdop / 10; - ao_gps_data.hdop = nav_dop.hdop / 10; - ao_gps_data.vdop = nav_dop.vdop / 10; + ao_gps_data.pdop = (uint8_t) (nav_dop.pdop / 10); + ao_gps_data.hdop = (uint8_t) (nav_dop.hdop / 10); + ao_gps_data.vdop = (uint8_t) (nav_dop.vdop / 10); - 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_data.ground_speed = (uint16_t) nav_velned.g_speed; + ao_gps_data.climb_rate = -(int16_t) nav_velned.vel_d; + ao_gps_data.course = (uint8_t) (nav_velned.heading / 200000); ao_gps_tracking_data.channels = 0; @@ -783,7 +792,7 @@ ao_gps(void) #if AO_UBLOX_DEBUG static void ao_gps_option(void) { - uint16_t r = ao_cmd_hex(); + uint8_t r = (uint8_t) ao_cmd_hex(); if (ao_cmd_status != ao_cmd_success) { ao_cmd_status = ao_cmd_success; ao_gps_show();