X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=2556d314547b08cd91a2a3971bf0166a7697a5ff;hp=24e70ee3a8ec34f711bb7115584a16fe1efa7749;hb=7f46240dfc57164f0c1b0c4c4ed9695bca63860d;hpb=5e9193f6375be27e5f7a0321fd34b6acfe81247f diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 24e70ee3..2556d314 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 @@ -21,31 +22,73 @@ #include "ao_gps_ublox.h" -__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; +#define AO_UBLOX_DEBUG 0 -static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n"; +#include -const char ao_gps_config[] = { +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 + +#ifndef AO_SERIAL_SPEED_UBLOX +#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600 +#endif + +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600 +#define SERIAL_SPEED_STRING "57600" +#define SERIAL_SPEED_CHECKSUM "2d" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200 +#define SERIAL_SPEED_STRING "19200" +#define SERIAL_SPEED_CHECKSUM "23" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600 +#define SERIAL_SPEED_STRING "9600" +#define SERIAL_SPEED_CHECKSUM "16" +#endif + +static const char ao_gps_set_nmea[] = + "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n"; 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 + +static uint8_t ao_gps_dbg_enable; + +#define DBG_PROTO 1 +#define DBG_CHAR 2 +#define DBG_INIT 4 + +static void ao_gps_dbg(int level, char *format, ...) { + va_list a; + + if (level & ao_gps_dbg_enable) { + va_start(a, format); + vprintf(format, a); + va_end(a); + flush(); + } +} -#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 +#else +#define ao_gps_dbg(fmt, ...) #endif -#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar()) +static inline uint8_t ao_ublox_byte(void) { + uint8_t c = (uint8_t) ao_gps_getchar(); + ao_gps_dbg(DBG_CHAR, " %02x", c); + return c; +} static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c) { @@ -61,7 +104,8 @@ static void ao_ublox_init_cksum(void) static void ao_ublox_put_u8(uint8_t c) { add_cksum(&ao_ublox_cksum, c); - ao_ublox_putchar(c); + ao_gps_dbg(DBG_CHAR, " (%02x)", c); + ao_gps_putchar(c); } static void ao_ublox_put_i8(int8_t c) @@ -108,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(); @@ -122,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 ()); @@ -159,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; @@ -183,6 +227,7 @@ ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __r break; } } + ao_gps_dbg(DBG_PROTO, "\n"); } /* @@ -326,6 +371,7 @@ ao_ublox_parse_nav_svinfo(void) { uint8_t nsat; nav_svinfo_nsat = 0; + ao_ublox_parse(&nav_svinfo, nav_svinfo_packet); for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) { if (nsat < NAV_SVINFO_MAX_SAT) { @@ -334,6 +380,17 @@ ao_ublox_parse_nav_svinfo(void) ublox_discard(12); } } +#if AO_UBLOX_DEBUG + ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags); + for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++) + ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n", + nsat, + nav_svinfo_sat[nsat].chn, + nav_svinfo_sat[nsat].svid, + nav_svinfo_sat[nsat].flags, + nav_svinfo_sat[nsat].quality, + nav_svinfo_sat[nsat].cno); +#endif } /* @@ -402,45 +459,60 @@ ao_ublox_parse_nav_velned(void) */ static void -ao_gps_setup(void) +ao_gps_delay(void) { - uint8_t i, k; - ao_ublox_set_speed(AO_SERIAL_SPEED_9600); + uint8_t i; /* * A bunch of nulls so the start bit * is clear */ + for (i = 0; i < 64; i++) - ao_ublox_putchar(0x00); + ao_gps_putchar(0x00); +} + +static void +ao_gps_setup(void) +{ + uint8_t i, k; + + ao_delay(AO_SEC_TO_TICKS(3)); + + ao_gps_dbg(DBG_INIT, "Set speed 9600\n"); + ao_gps_set_speed(AO_SERIAL_SPEED_9600); /* * Send the baud-rate setting and protocol-setting * command three times */ - for (k = 0; k < 3; k++) + for (k = 0; k < 3; k++) { + ao_gps_delay(); + + ao_gps_dbg(DBG_INIT, "Send initial setting\n"); 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]); + } + + ao_gps_delay(); +#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600 + ao_gps_dbg(DBG_INIT, "Set speed high\n"); /* * 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_delay(); } 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_gps_putchar(0xb5); + ao_gps_putchar(0x62); ao_ublox_put_u8(class); ao_ublox_put_u8(id); ao_ublox_put_u8(len); @@ -450,8 +522,8 @@ ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len) 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); } static void @@ -530,11 +602,21 @@ 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; uint8_t i; + AO_TICK_TYPE packet_start_tick; + AO_TICK_TYPE solution_tick = 0; ao_gps_setup(); @@ -545,9 +627,8 @@ 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, UBLOX_CFG_NAV5_FIXMODE_3D, @@ -566,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; @@ -578,6 +661,8 @@ ao_gps(void) __reentrant ao_ublox_len = header_byte(); ao_ublox_len |= header_byte() << 8; + 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; @@ -598,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) @@ -618,8 +704,10 @@ ao_gps(void) __reentrant break; } - if (ao_ublox_len != 0) + if (ao_ublox_len != 0) { + ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len); continue; + } /* verify checksum and end sequence */ cksum.a = ao_ublox_byte(); @@ -628,25 +716,25 @@ ao_gps(void) __reentrant continue; switch (class) { - case 0x01: + case UBLOX_NAV: switch (id) { - case 0x21: + 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; 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; } 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; @@ -658,36 +746,38 @@ 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; ao_gps_data.course = nav_velned.heading / 200000; - + 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); - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); break; } break; @@ -695,12 +785,28 @@ ao_gps(void) __reentrant } } -__code struct ao_cmds ao_gps_cmds[] = { - { ao_gps_show, "g\0Display GPS" }, +#if AO_UBLOX_DEBUG +static void ao_gps_option(void) +{ + 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 = r; + printf ("gps debug set to %d\n", ao_gps_dbg_enable); + } +} +#else +#define ao_gps_option ao_gps_show +#endif + +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)