From a2192eda5e446361fbb4560dc1e7c362f1301d5c Mon Sep 17 00:00:00 2001 From: Brad Bosch Date: Mon, 14 Dec 2020 18:26:03 -0600 Subject: [PATCH] GCS_MAVLink: use RADIO_STATUS message txbuf field this avoids overrunning radio buffer in queued_param_send() --- libraries/GCS_MAVLink/GCS.h | 3 +++ libraries/GCS_MAVLink/GCS_Common.cpp | 2 ++ libraries/GCS_MAVLink/GCS_Param.cpp | 2 +- 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index a28a2d0674..fdce7698f5 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -334,6 +334,7 @@ public: virtual uint64_t capabilities() const; uint16_t get_stream_slowdown_ms() const { return stream_slowdown_ms; } + uint8_t get_last_txbuf() const { return last_txbuf; } MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us); @@ -571,6 +572,8 @@ private: // number of extra ms to add to slow things down for the radio uint16_t stream_slowdown_ms; + // last reported radio buffer percent available + uint8_t last_txbuf = 100; // perf counters AP_HAL::Util::perf_counter_t _perf_packet; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 7414c5ec9c..10c3088d92 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -650,6 +650,8 @@ void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg, bool log_rad last_radio_status.remrssi_ms = now; } + last_txbuf = packet.txbuf; + // use the state of the transmit buffer in the radio to // control the stream rate, giving us adaptive software // flow control diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index 58ec127e99..e35b82d424 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -71,7 +71,7 @@ GCS_MAVLINK::queued_param_send() } count -= async_replies_sent_count; - while (count && _queued_parameter != nullptr) { + while (count && _queued_parameter != nullptr && get_last_txbuf() > 50) { char param_name[AP_MAX_NAME_SIZE]; _queued_parameter->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);