forked from Archive/PX4-Autopilot
Merge branch 'ubx_no_debug' into inav_gps_delay
This commit is contained in:
commit
612b25711f
|
@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||||
_gps_position(gps_position),
|
_gps_position(gps_position),
|
||||||
_configured(false),
|
_configured(false),
|
||||||
_waiting_for_ack(false),
|
_waiting_for_ack(false),
|
||||||
|
_got_posllh(false),
|
||||||
|
_got_velned(false),
|
||||||
|
_got_timeutc(false),
|
||||||
_disable_cmd_last(0)
|
_disable_cmd_last(0)
|
||||||
{
|
{
|
||||||
decode_init();
|
decode_init();
|
||||||
|
@ -275,9 +278,10 @@ UBX::receive(unsigned timeout)
|
||||||
bool handled = false;
|
bool handled = false;
|
||||||
|
|
||||||
while (true) {
|
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 */
|
/* 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) {
|
if (ret < 0) {
|
||||||
/* something went wrong when polling */
|
/* something went wrong when polling */
|
||||||
|
@ -286,7 +290,10 @@ UBX::receive(unsigned timeout)
|
||||||
|
|
||||||
} else if (ret == 0) {
|
} else if (ret == 0) {
|
||||||
/* return success after short delay after receiving a packet or timeout after long delay */
|
/* 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;
|
return 1;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -438,6 +445,7 @@ UBX::handle_message()
|
||||||
|
|
||||||
_rate_count_lat_lon++;
|
_rate_count_lat_lon++;
|
||||||
|
|
||||||
|
_got_posllh = true;
|
||||||
ret = 1;
|
ret = 1;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -447,8 +455,8 @@ UBX::handle_message()
|
||||||
gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
|
gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
|
||||||
|
|
||||||
_gps_position->fix_type = packet->gpsFix;
|
_gps_position->fix_type = packet->gpsFix;
|
||||||
_gps_position->s_variance_m_s = packet->sAcc;
|
_gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s
|
||||||
_gps_position->p_variance_m = packet->pAcc;
|
_gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m
|
||||||
_gps_position->timestamp_variance = hrt_absolute_time();
|
_gps_position->timestamp_variance = hrt_absolute_time();
|
||||||
|
|
||||||
ret = 1;
|
ret = 1;
|
||||||
|
@ -482,6 +490,7 @@ UBX::handle_message()
|
||||||
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
|
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
|
||||||
_gps_position->timestamp_time = hrt_absolute_time();
|
_gps_position->timestamp_time = hrt_absolute_time();
|
||||||
|
|
||||||
|
_got_timeutc = true;
|
||||||
ret = 1;
|
ret = 1;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -557,6 +566,7 @@ UBX::handle_message()
|
||||||
|
|
||||||
_rate_count_vel++;
|
_rate_count_vel++;
|
||||||
|
|
||||||
|
_got_velned = true;
|
||||||
ret = 1;
|
ret = 1;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -397,6 +397,9 @@ private:
|
||||||
struct vehicle_gps_position_s *_gps_position;
|
struct vehicle_gps_position_s *_gps_position;
|
||||||
bool _configured;
|
bool _configured;
|
||||||
bool _waiting_for_ack;
|
bool _waiting_for_ack;
|
||||||
|
bool _got_posllh;
|
||||||
|
bool _got_velned;
|
||||||
|
bool _got_timeutc;
|
||||||
uint8_t _message_class_needed;
|
uint8_t _message_class_needed;
|
||||||
uint8_t _message_id_needed;
|
uint8_t _message_id_needed;
|
||||||
ubx_decode_state_t _decode_state;
|
ubx_decode_state_t _decode_state;
|
||||||
|
|
|
@ -190,8 +190,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
||||||
/* check if there is space in the buffer, let it overflow else */
|
/* check if there is space in the buffer, let it overflow else */
|
||||||
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
|
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
|
||||||
|
|
||||||
if (desired > buf_free) {
|
if (buf_free < desired) {
|
||||||
desired = buf_free;
|
/* we don't want to send anything just in half, so return */
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -222,6 +223,8 @@ Mavlink::Mavlink() :
|
||||||
_subscriptions(nullptr),
|
_subscriptions(nullptr),
|
||||||
_streams(nullptr),
|
_streams(nullptr),
|
||||||
_mission_pub(-1),
|
_mission_pub(-1),
|
||||||
|
_mode(MAVLINK_MODE_NORMAL),
|
||||||
|
_total_counter(0),
|
||||||
_verbose(false),
|
_verbose(false),
|
||||||
_forwarding_on(false),
|
_forwarding_on(false),
|
||||||
_passing_on(false),
|
_passing_on(false),
|
||||||
|
|
Loading…
Reference in New Issue