Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Ublox, always quickly set the rates until we have fetched the version

This commit is contained in:
Michael du Breuil 2017-02-15 16:20:21 -07:00 committed by Andrew Tridgell
parent 76dabf2dd9
commit 7b483a1460
3 changed files with 30 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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];

34
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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));
}
/*

8
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -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);