From 303e985668bdb9b8c410922f9ca334c049719f39 Mon Sep 17 00:00:00 2001 From: Brad Bosch Date: Wed, 24 Jan 2024 11:27:06 -0600 Subject: [PATCH 1/3] GCS_MAVLink: respect txbuf flow control for FTP messages This gives slow radio links a fighting chance of getting FTP bulk download working even when they use a baud rate which is much higher than their current bandwidth. This should eliminate the need to disable FTP for parameter download on slow to moderate speed radio links like mLRS and ELRS. It allows removal of a hack in mLRS which results in a decrease in parameter download time for 19 Hz mLRS from 45-60 seconds to 11-17 seconds. This should also be good news for the ELRS rc-mavlink branch. --- libraries/GCS_MAVLink/GCS_FTP.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 10725ad586a236..78ee2446f73da1 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -105,7 +105,7 @@ bool GCS_MAVLINK::send_ftp_reply(const pending_ftp &reply) send_banner(); } WITH_SEMAPHORE(comm_chan_lock(reply.chan)); - if (!HAVE_PAYLOAD_SPACE(chan, FILE_TRANSFER_PROTOCOL)) { + if (!HAVE_PAYLOAD_SPACE(chan, FILE_TRANSFER_PROTOCOL) || get_last_txbuf() < 33) { return false; } uint8_t payload[251] = {}; From 7e3d16a9d66dcb658d3d96b44a8c4e2d7d8054ef Mon Sep 17 00:00:00 2001 From: Brad Bosch Date: Sun, 28 Jan 2024 16:25:37 -0600 Subject: [PATCH 2/3] GCS_MAVLink: Make get_last_txbuf() safer Add check for stale radio_status to get_last_txbuf() Move last_txbuf into last_radio_status struct --- libraries/GCS_MAVLink/GCS.h | 4 ++-- libraries/GCS_MAVLink/GCS_Common.cpp | 11 ++++++++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index abcdf00cbdfc0c..a350171554ae63 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -319,6 +319,7 @@ class GCS_MAVLINK return last_radio_status.remrssi_ms; } static float telemetry_radio_rssi(); // 0==no signal, 1==full signal + static uint8_t get_last_txbuf(); // mission item index to be sent on queued msg, delayed or not uint16_t mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE; @@ -481,7 +482,6 @@ class GCS_MAVLINK 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); @@ -768,6 +768,7 @@ class GCS_MAVLINK uint32_t remrssi_ms; uint8_t rssi; uint32_t received_ms; // time RADIO_STATUS received + uint8_t txbuf = 100; } last_radio_status; enum class Flags { @@ -814,7 +815,6 @@ class GCS_MAVLINK // 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; // outbound ("deferred message") queue. diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fe29ac468e513a..a320eb4eb8a492 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -833,6 +833,15 @@ float GCS_MAVLINK::telemetry_radio_rssi() return last_radio_status.rssi/254.0f; } +uint8_t GCS_MAVLINK::get_last_txbuf() +{ + if (AP_HAL::millis() - last_radio_status.received_ms > 5000) { + // stale report + return 100; + } + return last_radio_status.txbuf; +} + void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg) { mavlink_radio_t packet; @@ -849,7 +858,7 @@ void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg) last_radio_status.remrssi_ms = now; } - last_txbuf = packet.txbuf; + last_radio_status.txbuf = packet.txbuf; // use the state of the transmit buffer in the radio to // control the stream rate, giving us adaptive software From 268578eca6a29177e3ebf05028084feb70391bdd Mon Sep 17 00:00:00 2001 From: Brad Bosch Date: Mon, 5 Feb 2024 17:57:02 -0600 Subject: [PATCH 3/3] GCS_MAVLink: Cleanup and Reduce chance of GCS FTP timeout Delete unneeded orphan comment replace get_last_txbuf() with a predicate Make txbuf flow control threashold consistent between Parameter download and FTP and keep it in range where we are also slowing down normal streams Delay sending text banner until after first FTP response to reduce latency on slow links Don't let flow control delay setting ftp.last_send_ms so as to slow down normal streams as soon as possible to improve FTP response time --- libraries/GCS_MAVLink/GCS.h | 3 +-- libraries/GCS_MAVLink/GCS_Common.cpp | 8 +++---- libraries/GCS_MAVLink/GCS_FTP.cpp | 31 +++++++++++++++++----------- libraries/GCS_MAVLink/GCS_Param.cpp | 2 +- 4 files changed, 25 insertions(+), 19 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index a350171554ae63..a6231ed988b5c2 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -319,7 +319,7 @@ class GCS_MAVLINK return last_radio_status.remrssi_ms; } static float telemetry_radio_rssi(); // 0==no signal, 1==full signal - static uint8_t get_last_txbuf(); + static bool last_txbuf_is_greater(uint8_t txbuf_limit); // mission item index to be sent on queued msg, delayed or not uint16_t mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE; @@ -814,7 +814,6 @@ class GCS_MAVLINK // number of extra ms to add to slow things down for the radio uint16_t stream_slowdown_ms; - // last reported radio buffer percent available // outbound ("deferred message") queue. diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a320eb4eb8a492..3b81e551adbe47 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -833,13 +833,13 @@ float GCS_MAVLINK::telemetry_radio_rssi() return last_radio_status.rssi/254.0f; } -uint8_t GCS_MAVLINK::get_last_txbuf() +bool GCS_MAVLINK::last_txbuf_is_greater(uint8_t txbuf_limit) { if (AP_HAL::millis() - last_radio_status.received_ms > 5000) { // stale report - return 100; + return true; } - return last_radio_status.txbuf; + return last_radio_status.txbuf > txbuf_limit; } void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg) @@ -1173,7 +1173,7 @@ uint16_t GCS_MAVLINK::get_reschedule_interval_ms(const deferred_message_bucket_t interval_ms *= 4; } #if AP_MAVLINK_FTP_ENABLED - if (AP_HAL::millis() - ftp.last_send_ms < 500) { + if (AP_HAL::millis() - ftp.last_send_ms < 1000) { // we are sending ftp replies interval_ms *= 4; } diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 78ee2446f73da1..720142a839ef5c 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -97,15 +97,11 @@ void GCS_MAVLINK::handle_file_transfer_protocol(const mavlink_message_t &msg) { bool GCS_MAVLINK::send_ftp_reply(const pending_ftp &reply) { - /* - provide same banner we would give with old param download - */ - if (ftp.need_banner_send_mask & (1U<delay(2); } + + if (reply.req_opcode == FTP_OP::TerminateSession) { + ftp.last_send_ms = 0; + } + /* + provide same banner we would give with old param download + Do this after send_ftp_reply() to get the first FTP response out sooner + on slow links to avoid GCS timeout. The slowdown of normal streams in + get_reschedule_interval_ms() should help for subsequent responses. + */ + if (ftp.need_banner_send_mask & (1U< 50) { + while (count && _queued_parameter != nullptr && last_txbuf_is_greater(33)) { char param_name[AP_MAX_NAME_SIZE]; _queued_parameter->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);