mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
GPS: added auto-configuration of UBlox GPS
the Ublox will now auto-configure for 5Hz with just the messages we want. It also supports setting the navigation engine type
This commit is contained in:
Andrew Tridgell
parent
9c1ce9e1c5
commit
514be604a5
@ -31,7 +31,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
void
|
||||
AP_GPS_UBLOX::init(void)
|
||||
AP_GPS_UBLOX::init(enum GPS_Engine_Setting nav_setting)
|
||||
{
|
||||
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
|
||||
// right reporting configuration.
|
||||
@ -40,6 +40,11 @@ AP_GPS_UBLOX::init(void)
|
||||
|
||||
_epoch = TIME_OF_WEEK;
|
||||
idleTimeout = 1200;
|
||||
|
||||
// configure the GPS for the messages we want
|
||||
_configure_gps();
|
||||
|
||||
_nav_setting = nav_setting;
|
||||
}
|
||||
|
||||
// Process bytes available from the stream
|
||||
@ -99,38 +104,13 @@ AP_GPS_UBLOX::read(void)
|
||||
//
|
||||
case 2:
|
||||
_step++;
|
||||
if (CLASS_NAV == data) {
|
||||
_gather = true; // class is interesting, maybe gather
|
||||
} else {
|
||||
_gather = false; // class is not interesting, discard
|
||||
Debug("not interesting class=0x%x", data);
|
||||
}
|
||||
_class = data;
|
||||
_ck_b = _ck_a = data; // reset the checksum accumulators
|
||||
break;
|
||||
case 3:
|
||||
_step++;
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
_msg_id = data;
|
||||
if (_gather) { // if class was interesting
|
||||
switch(data) {
|
||||
case MSG_POSLLH: // message is interesting
|
||||
_expect = sizeof(ubx_nav_posllh);
|
||||
break;
|
||||
case MSG_STATUS:
|
||||
_expect = sizeof(ubx_nav_status);
|
||||
break;
|
||||
case MSG_SOL:
|
||||
_expect = sizeof(ubx_nav_solution);
|
||||
break;
|
||||
case MSG_VELNED:
|
||||
_expect = sizeof(ubx_nav_velned);
|
||||
break;
|
||||
default:
|
||||
_gather = false; // message is not interesting
|
||||
}
|
||||
} else {
|
||||
Debug("not interesting msg_id 0x%x", _msg_id);
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
_step++;
|
||||
@ -140,20 +120,24 @@ AP_GPS_UBLOX::read(void)
|
||||
case 5:
|
||||
_step++;
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
_payload_length += (uint16_t)data; // payload length high byte
|
||||
_payload_counter = 0; // prepare to receive payload
|
||||
if (_payload_length != _expect && _gather) {
|
||||
Debug("bad length _payload_length=%u _expect=%u", _payload_length, _expect);
|
||||
_gather = false;
|
||||
|
||||
_payload_length += (uint16_t)(data<<8);
|
||||
if (_payload_length > 512) {
|
||||
Debug("large payload %u", (unsigned)_payload_length);
|
||||
// assume very large payloads are line noise
|
||||
_payload_length = 0;
|
||||
_step = 0;
|
||||
}
|
||||
_payload_counter = 0; // prepare to receive payload
|
||||
break;
|
||||
|
||||
// Receive message data
|
||||
//
|
||||
case 6:
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
if (_gather) // gather data if requested
|
||||
if (_payload_counter < sizeof(_buffer)) {
|
||||
_buffer.bytes[_payload_counter] = data;
|
||||
}
|
||||
if (++_payload_counter == _payload_length)
|
||||
_step++;
|
||||
break;
|
||||
@ -174,12 +158,11 @@ AP_GPS_UBLOX::read(void)
|
||||
break; // bad checksum
|
||||
}
|
||||
|
||||
if (_gather && _parse_gps()) {
|
||||
if (_parse_gps()) {
|
||||
parsed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
Debug("parsed = %u", (unsigned)parsed);
|
||||
return parsed;
|
||||
}
|
||||
|
||||
@ -188,6 +171,39 @@ AP_GPS_UBLOX::read(void)
|
||||
bool
|
||||
AP_GPS_UBLOX::_parse_gps(void)
|
||||
{
|
||||
if (_class == CLASS_ACK) {
|
||||
Debug("ACK %u", (unsigned)_msg_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
|
||||
if (_nav_setting != GPS_ENGINE_NONE &&
|
||||
_buffer.nav_settings.dynModel != _nav_setting) {
|
||||
// we've received the current nav settings, change the engine
|
||||
// settings and send them back
|
||||
Debug("Changing engine setting from %u to %u\n",
|
||||
(unsigned)_buffer.nav_settings.dynModel, (unsigned)_nav_setting);
|
||||
_buffer.nav_settings.dynModel = _nav_setting;
|
||||
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
|
||||
&_buffer.nav_settings,
|
||||
sizeof(_buffer.nav_settings));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_class != CLASS_NAV) {
|
||||
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
|
||||
if (++_disable_counter == 0) {
|
||||
// disable future sends of this message id, but
|
||||
// only do this every 256 messages, as some
|
||||
// message types can't be disabled and we don't
|
||||
// want to get into an ack war
|
||||
Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
|
||||
_configure_message_rate(_class, _msg_id, 0);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (_msg_id) {
|
||||
case MSG_POSLLH:
|
||||
Debug("MSG_POSLLH next_fix=%u", next_fix);
|
||||
@ -226,6 +242,11 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
_new_speed = true;
|
||||
break;
|
||||
default:
|
||||
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
|
||||
if (++_disable_counter == 0) {
|
||||
Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
|
||||
_configure_message_rate(CLASS_NAV, _msg_id, 0);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -237,3 +258,99 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// UBlox auto configuration
|
||||
|
||||
/*
|
||||
update checksum for a set of bytes
|
||||
*/
|
||||
void
|
||||
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b)
|
||||
{
|
||||
while (len--) {
|
||||
ck_a += *data;
|
||||
ck_b += ck_a;
|
||||
data++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send a ublox message
|
||||
*/
|
||||
void
|
||||
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
|
||||
{
|
||||
struct ubx_header header;
|
||||
uint8_t ck_a=0, ck_b=0;
|
||||
header.preamble1 = PREAMBLE1;
|
||||
header.preamble2 = PREAMBLE2;
|
||||
header.msg_class = msg_class;
|
||||
header.msg_id = msg_id;
|
||||
header.length = size;
|
||||
|
||||
_update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
|
||||
_update_checksum((uint8_t *)msg, size, ck_a, ck_b);
|
||||
|
||||
_port->write((const uint8_t *)&header, sizeof(header));
|
||||
_port->write((const uint8_t *)msg, size);
|
||||
_port->write((const uint8_t *)&ck_a, 1);
|
||||
_port->write((const uint8_t *)&ck_b, 1);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
configure a UBlox GPS for the given message rate for a specific
|
||||
message class and msg_id
|
||||
*/
|
||||
void
|
||||
AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
|
||||
{
|
||||
struct ubx_cfg_msg_rate msg;
|
||||
msg.msg_class = msg_class;
|
||||
msg.msg_id = msg_id;
|
||||
msg.rate = rate;
|
||||
_send_message(CLASS_CFG, MSG_CFG_SET_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;
|
||||
|
||||
// this type 0x41 pubx sets us up for 38400 with
|
||||
// NMEA+UBX input and UBX output
|
||||
_send_pubx("$PUBX,41,1,0003,0001,38400,0");
|
||||
|
||||
// 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 the messages we parse to be sent on every navigation solution
|
||||
_configure_message_rate(CLASS_NAV, MSG_POSLLH, 1);
|
||||
_configure_message_rate(CLASS_NAV, MSG_STATUS, 1);
|
||||
_configure_message_rate(CLASS_NAV, MSG_SOL, 1);
|
||||
_configure_message_rate(CLASS_NAV, MSG_VELNED, 1);
|
||||
|
||||
// ask for the current navigation settings
|
||||
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
|
||||
}
|
||||
|
||||
void
|
||||
AP_GPS_UBLOX::_send_pubx(const char *msg)
|
||||
{
|
||||
uint8_t csum = 0;
|
||||
char suffix[4];
|
||||
for (uint8_t i=1; msg[i]; i++) {
|
||||
csum ^= msg[i];
|
||||
}
|
||||
_port->write(msg);
|
||||
sprintf(suffix, "*%02x", (unsigned)csum);
|
||||
_port->write((const uint8_t *)suffix, 3);
|
||||
}
|
||||
|
||||
@ -20,12 +20,48 @@ class AP_GPS_UBLOX : public GPS
|
||||
public:
|
||||
// Methods
|
||||
AP_GPS_UBLOX(Stream *s);
|
||||
virtual void init();
|
||||
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
|
||||
virtual bool read();
|
||||
|
||||
private:
|
||||
// u-blox UBX protocol essentials
|
||||
// XXX this is being ignored by the compiler #pragma pack(1)
|
||||
struct ubx_header {
|
||||
uint8_t preamble1;
|
||||
uint8_t preamble2;
|
||||
uint8_t msg_class;
|
||||
uint8_t msg_id;
|
||||
uint16_t length;
|
||||
};
|
||||
struct ubx_cfg_nav_rate {
|
||||
uint16_t measure_rate_ms;
|
||||
uint16_t nav_rate;
|
||||
uint16_t timeref;
|
||||
};
|
||||
struct ubx_cfg_msg_rate {
|
||||
uint8_t msg_class;
|
||||
uint8_t msg_id;
|
||||
uint8_t rate;
|
||||
};
|
||||
struct ubx_cfg_nav_settings {
|
||||
uint16_t mask;
|
||||
uint8_t dynModel;
|
||||
uint8_t fixMode;
|
||||
int32_t fixedAlt;
|
||||
uint32_t fixedAltVar;
|
||||
int8_t minElev;
|
||||
uint8_t drLimit;
|
||||
uint16_t pDop;
|
||||
uint16_t tDop;
|
||||
uint16_t pAcc;
|
||||
uint16_t tAcc;
|
||||
uint8_t staticHoldThresh;
|
||||
uint8_t res1;
|
||||
uint32_t res2;
|
||||
uint32_t res3;
|
||||
uint32_t res4;
|
||||
};
|
||||
|
||||
struct ubx_nav_posllh {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t longitude;
|
||||
@ -78,11 +114,18 @@ private:
|
||||
enum ubs_protocol_bytes {
|
||||
PREAMBLE1 = 0xb5,
|
||||
PREAMBLE2 = 0x62,
|
||||
CLASS_NAV = 0x1,
|
||||
CLASS_NAV = 0x01,
|
||||
CLASS_ACK = 0x05,
|
||||
CLASS_CFG = 0x06,
|
||||
MSG_ACK_NACK = 0x00,
|
||||
MSG_ACK_ACK = 0x01,
|
||||
MSG_POSLLH = 0x2,
|
||||
MSG_STATUS = 0x3,
|
||||
MSG_SOL = 0x6,
|
||||
MSG_VELNED = 0x12
|
||||
MSG_VELNED = 0x12,
|
||||
MSG_CFG_RATE = 0x08,
|
||||
MSG_CFG_SET_RATE = 0x01,
|
||||
MSG_CFG_NAV_SETTINGS = 0x24
|
||||
};
|
||||
enum ubs_nav_fix_type {
|
||||
FIX_NONE = 0,
|
||||
@ -103,23 +146,26 @@ private:
|
||||
// State machine state
|
||||
uint8_t _step;
|
||||
uint8_t _msg_id;
|
||||
bool _gather;
|
||||
uint16_t _expect;
|
||||
uint16_t _payload_length;
|
||||
uint16_t _payload_counter;
|
||||
|
||||
uint8_t _class;
|
||||
|
||||
// do we have new position information?
|
||||
bool _new_position;
|
||||
|
||||
// do we have new speed information?
|
||||
bool _new_speed;
|
||||
|
||||
uint8_t _disable_counter;
|
||||
|
||||
// Receive buffer
|
||||
union {
|
||||
ubx_nav_posllh posllh;
|
||||
ubx_nav_status status;
|
||||
ubx_nav_solution solution;
|
||||
ubx_nav_velned velned;
|
||||
ubx_cfg_nav_settings nav_settings;
|
||||
uint8_t bytes[];
|
||||
} _buffer;
|
||||
|
||||
@ -128,6 +174,13 @@ private:
|
||||
|
||||
// used to update fix between status and position packets
|
||||
bool next_fix;
|
||||
|
||||
void _send_pubx(const char *msg);
|
||||
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);
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user