mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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
This commit is contained in:
parent
bbfb0ed001
commit
6538e8c9ae
@ -319,7 +319,7 @@ public:
|
||||
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 @@ 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
|
||||
|
||||
// outbound ("deferred message") queue.
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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<<reply.chan)) {
|
||||
ftp.need_banner_send_mask &= ~(1U<<reply.chan);
|
||||
send_banner();
|
||||
if (!last_txbuf_is_greater(33)) { // It helps avoid GCS timeout if this is less than the threshold where we slow down normal streams (<=49)
|
||||
return false;
|
||||
}
|
||||
WITH_SEMAPHORE(comm_chan_lock(reply.chan));
|
||||
if (!HAVE_PAYLOAD_SPACE(chan, FILE_TRANSFER_PROTOCOL) || get_last_txbuf() < 33) {
|
||||
if (!HAVE_PAYLOAD_SPACE(chan, FILE_TRANSFER_PROTOCOL)) {
|
||||
return false;
|
||||
}
|
||||
uint8_t payload[251] = {};
|
||||
@ -121,11 +117,6 @@ bool GCS_MAVLINK::send_ftp_reply(const pending_ftp &reply)
|
||||
reply.chan,
|
||||
0, reply.sysid, reply.compid,
|
||||
payload);
|
||||
if (reply.req_opcode == FTP_OP::TerminateSession) {
|
||||
ftp.last_send_ms = 0;
|
||||
} else {
|
||||
ftp.last_send_ms = AP_HAL::millis();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -155,9 +146,25 @@ void GCS_MAVLINK::ftp_error(struct pending_ftp &response, FTP_ERROR error) {
|
||||
// send our response back out to the system
|
||||
void GCS_MAVLINK::ftp_push_replies(pending_ftp &reply)
|
||||
{
|
||||
ftp.last_send_ms = AP_HAL::millis(); // Used to detect active FTP session
|
||||
|
||||
while (!send_ftp_reply(reply)) {
|
||||
hal.scheduler->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<<reply.chan)) {
|
||||
ftp.need_banner_send_mask &= ~(1U<<reply.chan);
|
||||
send_banner();
|
||||
}
|
||||
}
|
||||
|
||||
void GCS_MAVLINK::ftp_worker(void) {
|
||||
|
@ -75,7 +75,7 @@ GCS_MAVLINK::queued_param_send()
|
||||
}
|
||||
count -= async_replies_sent_count;
|
||||
|
||||
while (count && _queued_parameter != nullptr && get_last_txbuf() > 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);
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user