diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 6116ca2500..774bc6109b 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -498,7 +498,7 @@ AP_GPS::update_instance(uint8_t instance) // has expired, re-initialise the GPS. This will cause GPS // detection to run again if (!result) { - if (tnow - timing[instance].last_message_time_ms > 2000) { + if (tnow - timing[instance].last_message_time_ms > 4000) { // free the driver before we run the next detection, so we // don't end up with two allocated at any time delete drivers[instance]; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 24cbc0b262..2374590381 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -105,7 +105,9 @@ AP_GPS_UBLOX::_request_next_config(void) case STEP_POLL_SVINFO: // not required once we know what generation we are on if(_hardware_generation == 0) { - _send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0); + if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) { + _next_message--; + } } break; case STEP_POLL_SBAS: @@ -116,13 +118,19 @@ AP_GPS_UBLOX::_request_next_config(void) } break; case STEP_POLL_NAV: - _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, nullptr, 0); + if (!_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, nullptr, 0)) { + _next_message--; + } break; case STEP_POLL_GNSS: - _send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0); + if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) { + _next_message--; + } break; case STEP_NAV_RATE: - _send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0); + if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) { + _next_message--; + } break; case STEP_POSLLH: if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) { @@ -345,7 +353,11 @@ AP_GPS_UBLOX::read(void) _request_next_config(); _last_config_time = millis_now; if (_unconfigured_messages) { // send the updates faster until fully configured - _delay_time = 750; + if (!havePvtMsg && (_unconfigured_messages & CONFIG_REQUIRED_INITIAL)) { + _delay_time = 300; + } else { + _delay_time = 750; + } } else { _delay_time = 2000; } @@ -1102,9 +1114,12 @@ AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8 /* * send a ublox message */ -void +bool AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size) { + if (port->txspace() < (sizeof(struct ubx_header) + 2 + size)) { + return false; + } struct ubx_header header; uint8_t ck_a=0, ck_b=0; header.preamble1 = PREAMBLE1; @@ -1120,6 +1135,7 @@ AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16 port->write((const uint8_t *)msg, size); port->write((const uint8_t *)&ck_a, 1); port->write((const uint8_t *)&ck_b, 1); + return true; } /* @@ -1139,8 +1155,7 @@ AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id) struct ubx_cfg_msg msg; msg.msg_class = msg_class; msg.msg_id = msg_id; - _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); - return true; + return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); } } @@ -1159,8 +1174,7 @@ AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t msg.msg_class = msg_class; msg.msg_id = msg_id; msg.rate = rate; - _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); - return true; + return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); } /* diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index cfbe13ddf4..b67d9f0954 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -74,6 +74,8 @@ #define CONFIG_SBAS (1<<12) #define CONFIG_RATE_PVT (1<<13) +#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) + #define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \ | CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \ | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS) @@ -476,16 +478,16 @@ private: enum config_step { STEP_PVT = 0, + STEP_NAV_RATE, // poll NAV rate STEP_SOL, STEP_PORT, - STEP_POSLLH, STEP_STATUS, + STEP_POSLLH, STEP_VELNED, STEP_POLL_SVINFO, // poll svinfo STEP_POLL_SBAS, // poll SBAS STEP_POLL_NAV, // poll NAV settings STEP_POLL_GNSS, // poll GNSS - STEP_NAV_RATE, // poll NAV rate STEP_DOP, STEP_MON_HW, STEP_MON_HW2, @@ -544,7 +546,7 @@ private: void _configure_rate(void); void _configure_sbas(bool enable); void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b); - void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size); + bool _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size); void send_next_rate_update(void); bool _request_message_rate(uint8_t msg_class, uint8_t msg_id); void _request_next_config(void);