diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-30 22:13:57 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-30 22:13:57 +0200 |
commit | 612b25711f6c24abbddf0be84adc3368e73c0d43 (patch) | |
tree | d8d61141a3ba64c3ad3837459e3edfef531cc994 /src | |
parent | 0bdde089243bbf7e3651f9535f5c847f894e0ac7 (diff) | |
parent | efb44969d584e1b2613d96a795fc94a3ef728d9a (diff) | |
download | px4-firmware-612b25711f6c24abbddf0be84adc3368e73c0d43.tar.gz px4-firmware-612b25711f6c24abbddf0be84adc3368e73c0d43.tar.bz2 px4-firmware-612b25711f6c24abbddf0be84adc3368e73c0d43.zip |
Merge branch 'ubx_no_debug' into inav_gps_delay
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/gps/ubx.cpp | 18 | ||||
-rw-r--r-- | src/drivers/gps/ubx.h | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 7 |
3 files changed, 22 insertions, 6 deletions
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..c143eeb0c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -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(); @@ -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; } @@ -447,8 +455,8 @@ UBX::handle_message() gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s + _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; @@ -482,6 +490,7 @@ 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; } @@ -557,6 +566,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b..43d688893 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -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; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb1ad86ef..28cdf65a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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 */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - if (desired > buf_free) { - desired = buf_free; + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + return; } } @@ -222,6 +223,8 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), |