Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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 // has expired, re-initialise the GPS. This will cause GPS
// detection to run again // detection to run again
if (!result) { 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 // free the driver before we run the next detection, so we
// don't end up with two allocated at any time // don't end up with two allocated at any time
delete drivers[instance]; 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: case STEP_POLL_SVINFO:
// not required once we know what generation we are on // not required once we know what generation we are on
if(_hardware_generation == 0) { 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; break;
case STEP_POLL_SBAS: case STEP_POLL_SBAS:
@ -116,13 +118,19 @@ AP_GPS_UBLOX::_request_next_config(void)
} }
break; break;
case STEP_POLL_NAV: 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; break;
case STEP_POLL_GNSS: 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; break;
case STEP_NAV_RATE: 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; break;
case STEP_POSLLH: case STEP_POSLLH:
if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) { if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
@ -345,7 +353,11 @@ AP_GPS_UBLOX::read(void)
_request_next_config(); _request_next_config();
_last_config_time = millis_now; _last_config_time = millis_now;
if (_unconfigured_messages) { // send the updates faster until fully configured 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 { } else {
_delay_time = 2000; _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 * send a ublox message
*/ */
void bool
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size) 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; struct ubx_header header;
uint8_t ck_a=0, ck_b=0; uint8_t ck_a=0, ck_b=0;
header.preamble1 = PREAMBLE1; 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 *)msg, size);
port->write((const uint8_t *)&ck_a, 1); port->write((const uint8_t *)&ck_a, 1);
port->write((const uint8_t *)&ck_b, 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; struct ubx_cfg_msg msg;
msg.msg_class = msg_class; msg.msg_class = msg_class;
msg.msg_id = msg_id; msg.msg_id = msg_id;
_send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
return true;
} }
} }
@ -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_class = msg_class;
msg.msg_id = msg_id; msg.msg_id = msg_id;
msg.rate = rate; msg.rate = rate;
_send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
return true;
} }
/* /*

8
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -74,6 +74,8 @@
#define CONFIG_SBAS (1<<12) #define CONFIG_SBAS (1<<12)
#define CONFIG_RATE_PVT (1<<13) #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 \ #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_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
| CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS) | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
@ -476,16 +478,16 @@ private:
enum config_step { enum config_step {
STEP_PVT = 0, STEP_PVT = 0,
STEP_NAV_RATE, // poll NAV rate
STEP_SOL, STEP_SOL,
STEP_PORT, STEP_PORT,
STEP_POSLLH,
STEP_STATUS, STEP_STATUS,
STEP_POSLLH,
STEP_VELNED, STEP_VELNED,
STEP_POLL_SVINFO, // poll svinfo STEP_POLL_SVINFO, // poll svinfo
STEP_POLL_SBAS, // poll SBAS STEP_POLL_SBAS, // poll SBAS
STEP_POLL_NAV, // poll NAV settings STEP_POLL_NAV, // poll NAV settings
STEP_POLL_GNSS, // poll GNSS STEP_POLL_GNSS, // poll GNSS
STEP_NAV_RATE, // poll NAV rate
STEP_DOP, STEP_DOP,
STEP_MON_HW, STEP_MON_HW,
STEP_MON_HW2, STEP_MON_HW2,
@ -544,7 +546,7 @@ private:
void _configure_rate(void); void _configure_rate(void);
void _configure_sbas(bool enable); void _configure_sbas(bool enable);
void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b); 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); void send_next_rate_update(void);
bool _request_message_rate(uint8_t msg_class, uint8_t msg_id); bool _request_message_rate(uint8_t msg_class, uint8_t msg_id);
void _request_next_config(void); void _request_next_config(void);