From 34f7f88190b6513ab114fc6ec6ef55db319295a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Oct 2013 12:32:32 +1000 Subject: [PATCH] AP_GPS: check for corret GPS solution rates in flight and fix if the GPS is giving us data at less than 300ms intervals then re-send the configuration messages to try to kick it into the right rates Pair-Programmed-With: Randy Mackay --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 75 ++++++++++++++++++++++++++++--- libraries/AP_GPS/AP_GPS_UBLOX.h | 10 ++++- 2 files changed, 78 insertions(+), 7 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index e3fb464b03..efbeaebd5c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -64,6 +64,45 @@ AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting) _new_speed = false; } +/* + send the next step of rate updates to the GPS. This reconfigures the + GPS on the fly to have the right message rates. It needs to be + careful to only send a message if there is sufficient buffer space + available on the serial port to avoid it blocking the CPU + */ +void +AP_GPS_UBLOX::send_next_rate_update(void) +{ + if (_port->txspace() < sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2) { + // not enough space - do it next time + return; + } + + switch (rate_update_step) { + case 0: + _configure_navigation_rate(200); + break; + case 1: + _configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); + break; + case 2: + _configure_message_rate(CLASS_NAV, MSG_STATUS, 1); + break; + case 3: + _configure_message_rate(CLASS_NAV, MSG_SOL, 1); + break; + case 4: + _configure_message_rate(CLASS_NAV, MSG_VELNED, 1); + break; + } + rate_update_step++; + if (rate_update_step > 4) { + need_rate_update = false; + rate_update_step = 0; + } +} + + // Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it @@ -80,6 +119,10 @@ AP_GPS_UBLOX::read(void) int16_t numc; bool parsed = false; + if (need_rate_update) { + send_next_rate_update(); + } + numc = _port->available(); for (int16_t i = 0; i < numc; i++) { // Process bytes received @@ -299,6 +342,17 @@ AP_GPS_UBLOX::_parse_gps(void) if (_new_position && _new_speed) { _new_speed = _new_position = false; _fix_count++; + uint32_t new_fix_time = hal.scheduler->millis(); + + if (!need_rate_update && new_fix_time - _last_fix_time > 300 && _fix_count % 20 == 0) { + // the GPS is running slow. It possibly browned out and + // restarted with incorrect parameters. We will slowly + // send out new parameters to fix it + need_rate_update = true; + rate_update_step = 0; + } + _last_fix_time = new_fix_time; + if (_fix_count == 100) { // ask for nav settings every 20 seconds Debug("Asking for engine setting\n"); @@ -365,13 +419,25 @@ AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t _send_message(CLASS_CFG, MSG_CFG_SET_RATE, &msg, sizeof(msg)); } +/* + * configure a UBlox GPS navigation solution rate of 200ms + */ +void +AP_GPS_UBLOX::_configure_navigation_rate(uint16_t rate_ms) +{ + struct ubx_cfg_nav_rate msg; + msg.measure_rate_ms = rate_ms; + msg.nav_rate = 1; + msg.timeref = 0; // UTC time + _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg)); +} + /* * configure a UBlox GPS for the given message rate */ void AP_GPS_UBLOX::_configure_gps(void) { - struct ubx_cfg_nav_rate msg; const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U}; // the GPS may be setup for a different baud rate. This ensures @@ -385,11 +451,8 @@ AP_GPS_UBLOX::_configure_gps(void) } _port->begin(38400U); - // ask for navigation solutions every 200ms - msg.measure_rate_ms = 200; - msg.nav_rate = 1; - msg.timeref = 0; // UTC time - _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg)); + // ask for nav solutions at 5Hz + _configure_navigation_rate(200); // ask for the messages we parse to be sent on every navigation solution _configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 145c5bb191..0093f4a91c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -51,7 +51,9 @@ public: _payload_counter(0), _fix_count(0), _disable_counter(0), - next_fix(GPS::FIX_NONE) + next_fix(GPS::FIX_NONE), + need_rate_update(false), + rate_update_step(0) {} // Methods @@ -219,10 +221,16 @@ private: // used to update fix between status and position packets Fix_Status next_fix; + bool need_rate_update; + uint8_t rate_update_step; + uint32_t _last_fix_time; + + void _configure_navigation_rate(uint16_t rate_ms); void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); void _configure_gps(void); void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b); void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); + void send_next_rate_update(void); };