X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=a6d930835baed49a70d619073b84349242b0a806;hp=4fb907462c43691807e355a21549b63a6dd1658b;hb=c417ab1de2a083b5fcff2e081e4feb2a65887903;hpb=ba99630f33440b993c69830856d2a7741ffdef71 diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 4fb90746..a6d93083 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; +uint16_t 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 @@ -151,11 +152,11 @@ 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(); @@ -165,16 +166,16 @@ static void ublox_u16(uint8_t offset) 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; @@ -601,7 +602,15 @@ static const uint8_t ublox_enable_nav[] = { }; void -ao_gps(void) __reentrant +ao_gps_set_rate(uint8_t rate) +{ + uint8_t i; + for (i = 0; i < sizeof (ublox_enable_nav); i++) + ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate); +} + +void +ao_gps(void) { uint8_t class, id; struct ao_ublox_cksum cksum; @@ -616,8 +625,7 @@ ao_gps(void) __reentrant ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0); /* Enable all of the messages we want */ - for (i = 0; i < sizeof (ublox_enable_nav); i++) - ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1); + ao_gps_set_rate(1); ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE), UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G, @@ -713,7 +721,7 @@ ao_gps(void) __reentrant ao_gps_data.flags |= AO_GPS_RUNNING; if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) { uint8_t nsat = nav_sol.nsat; - ao_gps_data.flags |= AO_GPS_VALID; + ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID; if (nsat > 15) nsat = 15; ao_gps_data.flags |= nsat; @@ -721,7 +729,7 @@ ao_gps(void) __reentrant if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC)) ao_gps_data.flags |= AO_GPS_DATE_VALID; - ao_gps_data.altitude = nav_posllh.alt_msl / 1000; + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000); ao_gps_data.latitude = nav_posllh.lat; ao_gps_data.longitude = nav_posllh.lon; @@ -733,11 +741,11 @@ ao_gps(void) __reentrant ao_gps_data.minute = nav_timeutc.min; ao_gps_data.second = nav_timeutc.sec; - ao_gps_data.pdop = nav_dop.pdop; - ao_gps_data.hdop = nav_dop.hdop; - ao_gps_data.vdop = nav_dop.vdop; - - /* mode is not set */ + /* 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.ground_speed = nav_velned.g_speed; ao_gps_data.climb_rate = -nav_velned.vel_d; @@ -746,18 +754,20 @@ ao_gps(void) __reentrant ao_gps_tracking_data.channels = 0; struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0]; + struct nav_svinfo_sat *src = &nav_svinfo_sat[0]; for (i = 0; i < nav_svinfo_nsat; i++) { - struct nav_svinfo_sat *src = &nav_svinfo_sat[i]; - if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) && src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED) { - dst->svid = src->svid; - dst->c_n_1 = src->cno; - dst++; - ao_gps_tracking_data.channels++; + if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) { + dst->svid = src->svid; + dst->c_n_1 = src->cno; + dst++; + ao_gps_tracking_data.channels++; + } } + src++; } ao_mutex_put(&ao_gps_mutex); @@ -773,12 +783,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); } } @@ -786,12 +796,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)