diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index abcdf00cbdfc0..a6231ed988b5c 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 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; @@ -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 { @@ -813,8 +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 - 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 fe29ac468e513..3b81e551adbe4 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; } +bool GCS_MAVLINK::last_txbuf_is_greater(uint8_t txbuf_limit) +{ + if (AP_HAL::millis() - last_radio_status.received_ms > 5000) { + // stale report + return true; + } + return last_radio_status.txbuf > txbuf_limit; +} + 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 @@ -1164,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 10725ad586a23..720142a839ef5 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -97,12 +97,8 @@ 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);