fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 09:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'gps_vel' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2013-05-09 16:09:33 +02:00
parent 614bbb1510 27a25b5e17
commit 5886e93a33
7 changed files with 205 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ubx.cpp
+97 -67
View File
@@ -60,7 +60,8 @@
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_waiting_for_ack(false)
_waiting_for_ack(false),
_disable_cmd_counter(0)
{
decode_init();
}
@@ -139,12 +140,12 @@ UBX::configure(unsigned &baudrate)
cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
@@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate)
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.clsID = UBX_CLASS_CFG;
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
/* For satelites info 1Hz is enough */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
/* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
// 0);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
0);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
_waiting_for_ack = false;
return 0;
@@ -239,6 +207,15 @@ UBX::configure(unsigned &baudrate)
return -1;
}
int
UBX::wait_for_ack(unsigned timeout)
{
_waiting_for_ack = true;
int ret = receive(timeout);
_waiting_for_ack = false;
return ret;
}
int
UBX::receive(unsigned timeout)
{
@@ -498,6 +475,8 @@ UBX::handle_message()
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
_rate_count_lat_lon++;
/* Add timestamp to finish the report */
_gps_position->timestamp_position = hrt_absolute_time();
/* only return 1 when new position is available */
@@ -653,6 +632,8 @@ UBX::handle_message()
_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->vel_ned_valid = true;
_gps_position->timestamp_velocity = hrt_absolute_time();
_rate_count_vel++;
}
break;
@@ -693,6 +674,12 @@ UBX::handle_message()
default: //we don't know the message
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
if (_disable_cmd_counter++ == 0) {
// Don't attempt for every message to disable, some might not be disabled */
warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
configure_message_rate(_message_class, _message_id, 0);
}
return ret;
ret = -1;
break;
}
@@ -736,6 +723,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
message[length-1] = ck_b;
}
void
UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
{
for (unsigned i = 0; i < length; i++) {
ck_a = ck_a + message[i];
ck_b = ck_b + ck_a;
}
}
void
UBX::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(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
}
void
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{
@@ -753,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
warnx("ubx: config write fail");
}
void
UBX::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.sync1 = UBX_SYNC1;
header.sync2 = UBX_SYNC2;
header.msg_class = msg_class;
header.msg_id = msg_id;
header.length = size;
add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
add_checksum((uint8_t *)msg, size, ck_a, ck_b);
// Configure receive check
_clsID_needed = msg_class;
_msgID_needed = msg_id;
write(_fd, (const char *)&header, sizeof(header));
write(_fd, (const char *)msg, size);
write(_fd, (const char *)&ck_a, 1);
write(_fd, (const char *)&ck_b, 1);
}