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

ubx: send update only if got POSLLH & VELNED & TIMEUTC

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-30 21:57:48 +02:00
parent 47c9d32620
commit efb44969d5
2 changed files with 100 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ubx.cpp
+97 -87
View File
@@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_gps_position(gps_position),
_configured(false),
_waiting_for_ack(false),
_got_posllh(false),
_got_velned(false),
_got_timeutc(false),
_disable_cmd_last(0)
{
decode_init();
@@ -219,19 +222,19 @@ UBX::configure(unsigned &baudrate)
return 1;
}
// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
//
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
// warnx("MSG CFG FAIL: NAV SVINFO");
// return 1;
// }
//
// configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
//
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
// warnx("MSG CFG FAIL: MON HW");
// return 1;
// }
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SVINFO");
return 1;
}
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: MON HW");
return 1;
}
_configured = true;
return 0;
@@ -275,9 +278,10 @@ UBX::receive(unsigned timeout)
bool handled = false;
while (true) {
bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
if (ret < 0) {
/* something went wrong when polling */
@@ -286,7 +290,10 @@ UBX::receive(unsigned timeout)
} else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */
if (handled) {
if (ready_to_return) {
_got_posllh = false;
_got_velned = false;
_got_timeutc = false;
return 1;
} else {
@@ -438,6 +445,7 @@ UBX::handle_message()
_rate_count_lat_lon++;
_got_posllh = true;
ret = 1;
break;
}
@@ -482,65 +490,66 @@ UBX::handle_message()
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
_gps_position->timestamp_time = hrt_absolute_time();
_got_timeutc = true;
ret = 1;
break;
}
// case UBX_MESSAGE_NAV_SVINFO: {
// //printf("GOT NAV_SVINFO\n");
// const int length_part1 = 8;
// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
// const int length_part2 = 12;
// gps_bin_nav_svinfo_part2_packet_t *packet_part2;
//
// uint8_t satellites_used = 0;
// int i;
//
// //printf("Number of Channels: %d\n", packet_part1->numCh);
// for (i = 0; i < packet_part1->numCh; i++) {
// /* set pointer to sattelite_i information */
// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
//
// /* write satellite information to global storage */
// uint8_t sv_used = packet_part2->flags & 0x01;
//
// if (sv_used) {
// /* count SVs used for NAV */
// satellites_used++;
// }
//
// /* record info for all channels, whether or not the SV is used for NAV */
// _gps_position->satellite_used[i] = sv_used;
// _gps_position->satellite_snr[i] = packet_part2->cno;
// _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
// _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
// _gps_position->satellite_prn[i] = packet_part2->svid;
// //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
// }
//
// for (i = packet_part1->numCh; i < 20; i++) {
// /* unused channels have to be set to zero for e.g. MAVLink */
// _gps_position->satellite_prn[i] = 0;
// _gps_position->satellite_used[i] = 0;
// _gps_position->satellite_snr[i] = 0;
// _gps_position->satellite_elevation[i] = 0;
// _gps_position->satellite_azimuth[i] = 0;
// }
//
// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
//
// if (packet_part1->numCh > 0) {
// _gps_position->satellite_info_available = true;
//
// } else {
// _gps_position->satellite_info_available = false;
// }
//
// _gps_position->timestamp_satellites = hrt_absolute_time();
//
// ret = 1;
// break;
// }
case UBX_MESSAGE_NAV_SVINFO: {
//printf("GOT NAV_SVINFO\n");
const int length_part1 = 8;
gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
const int length_part2 = 12;
gps_bin_nav_svinfo_part2_packet_t *packet_part2;
uint8_t satellites_used = 0;
int i;
//printf("Number of Channels: %d\n", packet_part1->numCh);
for (i = 0; i < packet_part1->numCh; i++) {
/* set pointer to sattelite_i information */
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
/* write satellite information to global storage */
uint8_t sv_used = packet_part2->flags & 0x01;
if (sv_used) {
/* count SVs used for NAV */
satellites_used++;
}
/* record info for all channels, whether or not the SV is used for NAV */
_gps_position->satellite_used[i] = sv_used;
_gps_position->satellite_snr[i] = packet_part2->cno;
_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
_gps_position->satellite_prn[i] = packet_part2->svid;
//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
}
for (i = packet_part1->numCh; i < 20; i++) {
/* unused channels have to be set to zero for e.g. MAVLink */
_gps_position->satellite_prn[i] = 0;
_gps_position->satellite_used[i] = 0;
_gps_position->satellite_snr[i] = 0;
_gps_position->satellite_elevation[i] = 0;
_gps_position->satellite_azimuth[i] = 0;
}
_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
if (packet_part1->numCh > 0) {
_gps_position->satellite_info_available = true;
} else {
_gps_position->satellite_info_available = false;
}
_gps_position->timestamp_satellites = hrt_absolute_time();
ret = 1;
break;
}
case UBX_MESSAGE_NAV_VELNED: {
// printf("GOT NAV_VELNED\n");
@@ -557,6 +566,7 @@ UBX::handle_message()
_rate_count_vel++;
_got_velned = true;
ret = 1;
break;
}
@@ -573,23 +583,23 @@ UBX::handle_message()
break;
}
// case UBX_CLASS_MON: {
// switch (_message_id) {
// case UBX_MESSAGE_MON_HW: {
//
// struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
//
// _gps_position->noise_per_ms = p->noisePerMS;
// _gps_position->jamming_indicator = p->jamInd;
//
// ret = 1;
// break;
// }
//
// default:
// break;
// }
// }
case UBX_CLASS_MON: {
switch (_message_id) {
case UBX_MESSAGE_MON_HW: {
struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
_gps_position->noise_per_ms = p->noisePerMS;
_gps_position->jamming_indicator = p->jamInd;
ret = 1;
break;
}
default:
break;
}
}
default:
break;
src/drivers/gps/ubx.h
+3
View File
@@ -397,6 +397,9 @@ private:
struct vehicle_gps_position_s *_gps_position;
bool _configured;
bool _waiting_for_ack;
bool _got_posllh;
bool _got_velned;
bool _got_timeutc;
uint8_t _message_class_needed;
uint8_t _message_id_needed;
ubx_decode_state_t _decode_state;