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);