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

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 2012-06-10 16:34:13 +10:00
parent 9c1ce9e1c5
commit 514be604a5
2 changed files with 212 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

187
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

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

63
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

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