X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=57cbf22a77b50245975955925096ba85ac9723f5;hb=fadb563f8aeeee895a7232bb71fa5bade6ef66f0;hp=74c29e0a0e74e20af16436e66ffd2de3ab4d12b5;hpb=47e62bb26984f6c84660c1d0451f77c2d6ad7e5a;p=fw%2Faltos diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 74c29e0a..57cbf22a 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -3,7 +3,8 @@ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -25,11 +26,11 @@ #include -__xdata uint8_t ao_gps_new; -__xdata uint8_t ao_gps_mutex; -__pdata uint16_t ao_gps_tick; -__xdata struct ao_telemetry_location ao_gps_data; -__xdata struct ao_telemetry_satellite ao_gps_tracking_data; +uint8_t ao_gps_new; +uint8_t ao_gps_mutex; +AO_TICK_TYPE ao_gps_tick; +struct ao_telemetry_location ao_gps_data; +struct ao_telemetry_satellite ao_gps_tracking_data; #undef AO_SERIAL_SPEED_UBLOX @@ -57,8 +58,8 @@ struct ao_ublox_cksum { uint8_t a, b; }; -static __pdata struct ao_ublox_cksum ao_ublox_cksum; -static __pdata uint16_t ao_ublox_len; +static struct ao_ublox_cksum ao_ublox_cksum; +static uint16_t ao_ublox_len; #if AO_UBLOX_DEBUG @@ -114,8 +115,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 @@ -127,10 +128,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) @@ -151,30 +152,30 @@ static uint8_t data_byte(void) return header_byte(); } -static char __xdata *ublox_target; +static char *ublox_target; static void ublox_u16(uint8_t offset) { - uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset); + uint16_t *ptr = (uint16_t *) (void *) (ublox_target + offset); uint16_t val; val = data_byte(); - val |= data_byte () << 8; + val |= (uint16_t) ((uint16_t) data_byte () << 8); *ptr = val; } static void ublox_u8(uint8_t offset) { - uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset); + uint8_t *ptr = (uint8_t *) (ublox_target + offset); uint8_t val; val = data_byte (); *ptr = val; } -static void ublox_u32(uint8_t offset) __reentrant +static void ublox_u32(uint8_t offset) { - uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset); + uint32_t *ptr = (uint32_t *) (void *) (ublox_target + offset); uint32_t val; val = ((uint32_t) data_byte ()); @@ -202,7 +203,7 @@ struct ublox_packet_parse { }; static void -ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant +ao_ublox_parse(void *target, const struct ublox_packet_parse *parse) { uint8_t i, offset; @@ -514,8 +515,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 @@ -609,11 +610,13 @@ ao_gps_set_rate(uint8_t rate) } void -ao_gps(void) __reentrant +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(); @@ -644,6 +647,8 @@ ao_gps(void) __reentrant /* 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; @@ -654,9 +659,9 @@ ao_gps(void) __reentrant /* 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; @@ -678,6 +683,7 @@ ao_gps(void) __reentrant 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) @@ -714,7 +720,7 @@ ao_gps(void) __reentrant switch (id) { case UBLOX_NAV_TIMEUTC: ao_mutex_get(&ao_gps_mutex); - ao_gps_tick = ao_time(); + ao_gps_tick = solution_tick; ao_gps_data.flags = 0; ao_gps_data.flags |= AO_GPS_RUNNING; @@ -732,7 +738,7 @@ ao_gps(void) __reentrant 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; @@ -742,13 +748,13 @@ ao_gps(void) __reentrant /* 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; @@ -782,12 +788,12 @@ ao_gps(void) __reentrant #if AO_UBLOX_DEBUG static void ao_gps_option(void) { - ao_cmd_hex(); + uint16_t r = ao_cmd_hex(); if (ao_cmd_status != ao_cmd_success) { ao_cmd_status = ao_cmd_success; ao_gps_show(); } else { - ao_gps_dbg_enable = ao_cmd_lex_i; + ao_gps_dbg_enable = r; printf ("gps debug set to %d\n", ao_gps_dbg_enable); } } @@ -795,12 +801,12 @@ static void ao_gps_option(void) #define ao_gps_option ao_gps_show #endif -__code struct ao_cmds ao_gps_cmds[] = { +const struct ao_cmds ao_gps_cmds[] = { { ao_gps_option, "g\0Display GPS" }, { 0, NULL }, }; -__xdata struct ao_task ao_gps_task; +struct ao_task ao_gps_task; void ao_gps_init(void)