From 836ac48860620dfdecc57d1d3b5e5de3b766aa5a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 6 Aug 2018 11:25:05 -0400 Subject: [PATCH 1/4] Mavlink check transmit buffer before send - partially addresses #10389 --- src/modules/mavlink/mavlink_main.cpp | 28 +++++++++++++++++--------- src/modules/mavlink/mavlink_main.h | 2 ++ src/modules/mavlink/mavlink_stream.cpp | 4 +++- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index eebf92e685d7..58ba4b96fb33 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -964,6 +964,23 @@ Mavlink::send_packet() return ret; } +bool +Mavlink::should_send_bytes(unsigned packet_len) +{ + if (get_protocol() == SERIAL) { + /* check if there is space in the buffer, let it overflow else */ + unsigned buf_free = get_free_tx_buf(); + + if (buf_free < packet_len) { + /* not enough space in buffer to send */ + count_txerrbytes(packet_len); + return false; + } + } + + return true; +} + void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) { @@ -979,15 +996,8 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) _mavlink_start_time = _last_write_try_time; } - if (get_protocol() == SERIAL) { - /* check if there is space in the buffer, let it overflow else */ - unsigned buf_free = get_free_tx_buf(); - - if (buf_free < packet_len) { - /* not enough space in buffer to send */ - count_txerrbytes(packet_len); - return; - } + if (!should_send_bytes(packet_len)) { + return; } size_t ret = -1; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e2c21e556ef6..6111204aa07f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -307,6 +307,8 @@ class Mavlink : public ModuleParams */ void send_bytes(const uint8_t *buf, unsigned packet_len); + bool should_send_bytes(unsigned packet_len); + /** * Flush the transmit buffer and send one MAVLink packet * diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index af7c9fb3fdec..7b2ee972ae3b 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -105,7 +105,9 @@ MavlinkStream::update(const hrt_abstime &t) // do not use the actual time but increment at a fixed rate, so that processing delays do not // distort the average rate. The check of the maximum interval is done to ensure that after a // long time not sending anything, sending multiple messages in a short time is avoided. - if (send(t)) { + + // check TX buffer before doing anything + if (_mavlink->should_send_bytes(get_size()) && send(t)) { _last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t; if (!_first_message_sent) { From c75cbb57608ffb1872bc1823c78e36f23ea54d50 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Nov 2018 17:09:04 -0500 Subject: [PATCH 2/4] mavlink parameters respect data rate --- src/modules/logger/logger.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 33 ++++++++++++---------- src/modules/mavlink/mavlink_main.h | 17 +++++++---- src/modules/mavlink/mavlink_parameters.cpp | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 5 files changed, 34 insertions(+), 23 deletions(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index bf92004165fd..c205688532ba 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -642,7 +642,7 @@ void Logger::add_default_topics() add_topic("system_power", 500); add_topic("tecs_status", 200); add_topic("trajectory_setpoint"); - add_topic("telemetry_status"); + add_topic("telemetry_status", 1000); add_topic("vehicle_air_data", 200); add_topic("vehicle_attitude", 30); add_topic("vehicle_attitude_setpoint", 100); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 58ba4b96fb33..8b997dd9905d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -245,10 +245,6 @@ Mavlink::Mavlink() : _mavlink_start_time(0), _protocol_version_switch(-1), _protocol_version(0), - _bytes_tx(0), - _bytes_txerr(0), - _bytes_rx(0), - _bytes_timestamp(0), #if defined(CONFIG_NET) || defined(__PX4_POSIX) _myaddr {}, _src_addr{}, @@ -2552,20 +2548,19 @@ Mavlink::task_main(int argc, char *argv[]) } /* update TX/RX rates*/ - if (t > _bytes_timestamp + 1000000) { - if (_bytes_timestamp != 0) { - const float dt = (t - _bytes_timestamp) / 1000.0f; + const uint64_t elapsed = hrt_elapsed_time(&_bytes_timestamp); - _tstatus.rate_tx = _bytes_tx / dt; - _tstatus.rate_txerr = _bytes_txerr / dt; - _tstatus.rate_rx = _bytes_rx / dt; + if (elapsed >= 1_s) { + const float dt = elapsed / 1000.0f; - _bytes_tx = 0; - _bytes_txerr = 0; - _bytes_rx = 0; - } + _tstatus.rate_tx = _bytes_tx / dt; + _tstatus.rate_txerr = _bytes_txerr / dt; + _tstatus.rate_rx = _bytes_rx / dt; - _bytes_timestamp = t; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + _bytes_timestamp = hrt_absolute_time(); } // publish status at 1 Hz, or sooner if HEARTBEAT has updated @@ -2661,6 +2656,14 @@ void Mavlink::publish_telemetry_status() orb_publish_auto(ORB_ID(telemetry_status), &_telem_status_pub, &_tstatus, &instance, ORB_PRIO_DEFAULT); } +bool Mavlink::check_tx_rate() const +{ + const float dt = hrt_elapsed_time(&_bytes_timestamp) / 1e6f; + const float rate = _bytes_tx / dt; + + return (rate < (_datarate * _rate_mult)); +} + void Mavlink::check_radio_config() { /* radio config check */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6111204aa07f..9a27fca52190 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -260,6 +260,13 @@ class Mavlink : public ModuleParams */ unsigned get_free_tx_buf(); + /** + * Check if the transmit data rate has been exceeded + * + * @return true if the transmit rate is within the current allowable data rate + */ + bool check_tx_rate() const; + static int start_helper(int argc, char *argv[]); /** @@ -467,7 +474,7 @@ class Mavlink : public ModuleParams bool is_usb_uart() { return _is_usb_uart; } - int get_data_rate() { return _datarate; } + int get_data_rate() const { return _datarate; } void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } } unsigned get_main_loop_delay() const { return _main_loop_delay; } @@ -593,10 +600,10 @@ class Mavlink : public ModuleParams int32_t _protocol_version_switch; int32_t _protocol_version; - unsigned _bytes_tx; - unsigned _bytes_txerr; - unsigned _bytes_rx; - uint64_t _bytes_timestamp; + unsigned _bytes_tx{0}; + unsigned _bytes_txerr{0}; + unsigned _bytes_rx{0}; + uint64_t _bytes_timestamp{0}; #if defined(CONFIG_NET) || defined(__PX4_POSIX) struct sockaddr_in _myaddr; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b853648c2256..760af40b1acf 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -320,7 +320,8 @@ MavlinkParametersManager::send(const hrt_abstime t) int i = 0; // Send while burst is not exceeded, we still have buffer space and still something to send - while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && send_params()); + while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && _mavlink->check_tx_rate() + && send_params()); } bool diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b062aa9f08a9..186fa08d354c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2741,7 +2741,7 @@ MavlinkReceiver::receive_thread(void *arg) } } - hrt_abstime t = hrt_absolute_time(); + const hrt_abstime t = hrt_absolute_time(); if (t - last_send_update > timeout * 1000) { _mission_manager.check_active_mission(); From 8be15d6b7fa5511141d325f5aa3b09d879f575ca Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Nov 2018 17:34:19 -0500 Subject: [PATCH 3/4] mavlink reduce rate aggressively in the presence of rx errors --- src/modules/mavlink/mavlink_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8b997dd9905d..f07e6f0fe9f3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1707,13 +1707,17 @@ Mavlink::update_rate_mult() void Mavlink::update_radio_status(const radio_status_s &radio_status) { - _rstatus = radio_status; set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO); /* check hardware limits */ _radio_status_available = true; _radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE); + // radio rx errors indicate a significant problem, drop rate as a precaution + if (radio_status.rxerrors > _rstatus.rxerrors) { + _radio_status_mult *= 0.50f; + } + if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { /* this indicates link congestion, reduce rate by 20% */ _radio_status_mult *= 0.80f; @@ -1726,6 +1730,8 @@ Mavlink::update_radio_status(const radio_status_s &radio_status) /* this indicates spare bandwidth, increase by 2.5% */ _radio_status_mult *= 1.025f; } + + _rstatus = radio_status; } int From 3b6969232d2cd57be6bb372ba144a7803ff7c4e8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 10 Nov 2018 13:16:22 -0500 Subject: [PATCH 4/4] WIP --- msg/telemetry_status.msg | 4 +- src/modules/mavlink/mavlink_main.cpp | 95 +++++++++------------- src/modules/mavlink/mavlink_main.h | 23 ++---- src/modules/mavlink/mavlink_parameters.cpp | 3 +- 4 files changed, 52 insertions(+), 73 deletions(-) diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 2bc34ce4d888..324c0027aee2 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -24,8 +24,10 @@ bool ftp uint8 streams float32 data_rate +float32 current_data_rate -float32 rate_multiplier +float32 data_rate_multiplier +float32 error_rate_multiplier float32 rate_rx diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f07e6f0fe9f3..0e6ad166ffc5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -233,9 +233,7 @@ Mavlink::Mavlink() : _uart_fd(-1), _baudrate(57600), _datarate(1000), - _rate_mult(1.0f), _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _udp_initialised(false), @@ -974,6 +972,14 @@ Mavlink::should_send_bytes(unsigned packet_len) } } + const float dt = hrt_elapsed_time(&_bytes_timestamp) / 1e6f; + const float rate = (_bytes_tx + packet_len) / dt; + + if (rate > (get_allowable_data_rate())) { + count_txerrbytes(packet_len); + return false; + } + return true; } @@ -992,10 +998,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) _mavlink_start_time = _last_write_try_time; } - if (!should_send_bytes(packet_len)) { - return; - } - size_t ret = -1; /* send message to UART */ @@ -1669,39 +1671,15 @@ Mavlink::update_rate_mult() } /* scale up and down as the link permits */ - float bandwidth_mult = (float)(_datarate * mavlink_ulog_streaming_rate_inv - const_rate) / rate; + float bandwidth_mult = (float)(get_allowable_data_rate() * mavlink_ulog_streaming_rate_inv - const_rate) / rate; /* if we do not have flow control, limit to the set data rate */ if (!get_flow_control_enabled()) { - bandwidth_mult = fminf(1.0f, bandwidth_mult); + bandwidth_mult = math::constrain(bandwidth_mult, 0.0f, 1.0f); } - float hardware_mult = 1.0f; - - /* scale down if we have a TX err rate suggesting link congestion */ - if (_tstatus.rate_txerr > 0.0f && !_radio_status_critical) { - hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr); - - } else if (_radio_status_available) { - - // check for RADIO_STATUS timeout and reset - if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) { - PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); - set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC); - - _radio_status_available = false; - _radio_status_critical = false; - _radio_status_mult = 1.0f; - } - - hardware_mult *= _radio_status_mult; - } - - /* pick the minimum from bandwidth mult and hardware mult as limit */ - _rate_mult = fminf(bandwidth_mult, hardware_mult); - /* ensure the rate multiplier never drops below 5% so that something is always sent */ - _rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f); + _rate_mult = math::constrain(bandwidth_mult * get_error_rate_mult(), 0.05f, 1.0f); } void @@ -1711,26 +1689,27 @@ Mavlink::update_radio_status(const radio_status_s &radio_status) /* check hardware limits */ _radio_status_available = true; - _radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE); // radio rx errors indicate a significant problem, drop rate as a precaution if (radio_status.rxerrors > _rstatus.rxerrors) { - _radio_status_mult *= 0.50f; + _error_rate_mult *= 0.50f; } if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { /* this indicates link congestion, reduce rate by 20% */ - _radio_status_mult *= 0.80f; + _error_rate_mult *= 0.80f; } else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) { /* this indicates link congestion, reduce rate by 2.5% */ - _radio_status_mult *= 0.975f; + _error_rate_mult *= 0.975f; } else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) { /* this indicates spare bandwidth, increase by 2.5% */ - _radio_status_mult *= 1.025f; + _error_rate_mult *= 1.025f; } + _error_rate_mult = math::constrain(_error_rate_mult, 0.01f, 1.0f); + _rstatus = radio_status; } @@ -2069,10 +2048,6 @@ Mavlink::task_main(int argc, char *argv[]) break; #endif -// case 'e': -// mavlink_link_termination_allowed = true; -// break; - case 'm': { int mode; @@ -2292,8 +2267,6 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); - update_rate_mult(); - if (param_sub->update(¶m_time, nullptr)) { mavlink_update_parameters(); @@ -2486,6 +2459,8 @@ Mavlink::task_main(int argc, char *argv[]) _subscribe_to_stream = nullptr; } + update_rate_mult(); + /* update streams */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { @@ -2563,6 +2538,20 @@ Mavlink::task_main(int argc, char *argv[]) _tstatus.rate_txerr = _bytes_txerr / dt; _tstatus.rate_rx = _bytes_rx / dt; + // gradually increase data rate if there were no errors + if (_bytes_txerr == 0) { + _error_rate_mult += 0.05f; + + } else { + const float rate = static_cast(_bytes_tx) / static_cast(_bytes_tx + _bytes_txerr); + + if (PX4_ISFINITE(rate)) { + _error_rate_mult *= rate; + } + } + + _error_rate_mult = math::constrain(_error_rate_mult, 0.05f, 1.0f); + _bytes_tx = 0; _bytes_txerr = 0; _bytes_rx = 0; @@ -2640,8 +2629,10 @@ void Mavlink::publish_telemetry_status() // many fields are populated in place _tstatus.mode = _mode; - _tstatus.data_rate = _datarate; - _tstatus.rate_multiplier = _rate_mult; + _tstatus.data_rate = get_data_rate(); + _tstatus.current_data_rate = get_allowable_data_rate(); + _tstatus.data_rate_multiplier = get_rate_mult(); + _tstatus.error_rate_multiplier = get_error_rate_mult(); _tstatus.flow_control = get_flow_control_enabled(); _tstatus.ftp = ftp_enabled(); _tstatus.forwarding = get_forwarding_on(); @@ -2662,14 +2653,6 @@ void Mavlink::publish_telemetry_status() orb_publish_auto(ORB_ID(telemetry_status), &_telem_status_pub, &_tstatus, &instance, ORB_PRIO_DEFAULT); } -bool Mavlink::check_tx_rate() const -{ - const float dt = hrt_elapsed_time(&_bytes_timestamp) / 1e6f; - const float rate = _bytes_tx / dt; - - return (rate < (_datarate * _rate_mult)); -} - void Mavlink::check_radio_config() { /* radio config check */ @@ -2846,8 +2829,8 @@ Mavlink::display_status() printf("\trates:\n"); printf("\t tx: %.3f kB/s\n", (double)_tstatus.rate_tx); printf("\t txerr: %.3f kB/s\n", (double)_tstatus.rate_txerr); - printf("\t tx rate mult: %.3f\n", (double)_rate_mult); - printf("\t tx rate max: %i B/s\n", _datarate); + printf("\t tx rate mult: %.3f\n", (double)get_rate_mult()); + printf("\t tx rate max: %i B/s\n", get_data_rate()); printf("\t rx: %.3f kB/s\n", (double)_tstatus.rate_rx); if (_mavlink_ulog) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9a27fca52190..1b24bb5cc0d0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -59,11 +59,11 @@ #include #endif -#include +#include #include -#include #include #include +#include #include #include @@ -260,13 +260,6 @@ class Mavlink : public ModuleParams */ unsigned get_free_tx_buf(); - /** - * Check if the transmit data rate has been exceeded - * - * @return true if the transmit rate is within the current allowable data rate - */ - bool check_tx_rate() const; - static int start_helper(int argc, char *argv[]); /** @@ -401,6 +394,7 @@ class Mavlink : public ModuleParams MavlinkStream *get_streams() const { return _streams; } float get_rate_mult() const { return _rate_mult; } + float get_error_rate_mult() const { return _error_rate_mult; } float get_baudrate() { return _baudrate; } @@ -409,6 +403,7 @@ class Mavlink : public ModuleParams bool get_has_received_messages() { return _received_messages; } void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (_transmitting_enabled && _boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); } bool message_buffer_write(const void *ptr, int size); @@ -475,6 +470,8 @@ class Mavlink : public ModuleParams bool is_usb_uart() { return _is_usb_uart; } int get_data_rate() const { return _datarate; } + int get_allowable_data_rate() const { return _datarate * _error_rate_mult; } + void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } } unsigned get_main_loop_delay() const { return _main_loop_delay; } @@ -574,11 +571,11 @@ class Mavlink : public ModuleParams int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) - float _rate_mult; + + float _rate_mult{1.0f}; + float _error_rate_mult{1.0f}; bool _radio_status_available{false}; - bool _radio_status_critical{false}; - float _radio_status_mult{1.0f}; /** * If the queue index is not at 0, the queue sending @@ -587,8 +584,6 @@ class Mavlink : public ModuleParams */ unsigned int _mavlink_param_queue_index; - bool mavlink_link_termination_allowed; - char *_subscribe_to_stream; float _subscribe_to_stream_rate; ///< rate of stream to subscribe to (0=disable, -1=unlimited, -2=default) bool _udp_initialised; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 760af40b1acf..f9e751e9514c 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -320,8 +320,7 @@ MavlinkParametersManager::send(const hrt_abstime t) int i = 0; // Send while burst is not exceeded, we still have buffer space and still something to send - while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && _mavlink->check_tx_rate() - && send_params()); + while ((i++ < max_num_to_send) && _mavlink->should_send_bytes(get_size()) && send_params()); } bool