From 9e385b6afe7057105fefaada2bdee8475d287614 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 11 Nov 2023 10:39:23 +1300 Subject: [PATCH] core: fix burst implementation This fixes the burst implementation which was implemented according to the docs rather than the existing implementations out there (like ArduPilot, MissionPlanner, and QGroundControl. The difference is that the size field is used to signal the size of each burst packet rather than the overall burst. This means a burst transfer always has to be the whole file and can't be a partial file. Signed-off-by: Julian Oes --- src/mavsdk/core/mavlink_ftp_client.cpp | 124 ++++++++++++++++++++++--- src/mavsdk/core/mavlink_ftp_client.h | 4 +- src/mavsdk/core/mavlink_ftp_server.cpp | 26 ++---- src/mavsdk/core/mavlink_ftp_server.h | 2 +- 4 files changed, 125 insertions(+), 31 deletions(-) diff --git a/src/mavsdk/core/mavlink_ftp_client.cpp b/src/mavsdk/core/mavlink_ftp_client.cpp index 2b1f7187dc..cb5cb18a62 100644 --- a/src/mavsdk/core/mavlink_ftp_client.cpp +++ b/src/mavsdk/core/mavlink_ftp_client.cpp @@ -1,6 +1,7 @@ #include "mavlink_ftp_client.h" #include "system_impl.h" #include "plugin_base.h" +#include "unused.h" #include #include #include @@ -180,7 +181,8 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) [&](DownloadBurstItem& item) { if (payload->opcode == RSP_ACK) { if (payload->req_opcode == CMD_OPEN_FILE_RO || - payload->req_opcode == CMD_BURST_READ_FILE) { + payload->req_opcode == CMD_BURST_READ_FILE || + payload->req_opcode == CMD_READ_FILE) { // Whenever we do get an ack, // reset the retry counter. work->retries = RETRIES; @@ -506,7 +508,7 @@ bool MavlinkFtpClient::download_burst_continue( LogDebug() << "Burst Download continue, got file size: " << file_size; } - request_next_burst(work, item); + request_burst(work, item); return true; } else if (payload->req_opcode == CMD_BURST_READ_FILE) { @@ -535,8 +537,12 @@ bool MavlinkFtpClient::download_burst_continue( item.transferred[i] = DownloadBurstItem::Transferred::Yes; } - const size_t bytes_transferred = std::count( - item.transferred.begin(), item.transferred.end(), DownloadBurstItem::Transferred::Yes); + if (_debugging) { + LogDebug() << "Received " << payload->offset << " to " + << payload->size + payload->offset; + } + + const size_t bytes_transferred = burst_bytes_transferred(item); if (bytes_transferred == item.transferred.size()) { if (_debugging) { @@ -567,8 +573,9 @@ bool MavlinkFtpClient::download_burst_continue( static_cast(item.transferred.size())}); if (payload->burst_complete) { - // The burst is supposedly complete but we still need data, so request next burst. - request_next_burst(work, item); + // The burst is supposedly complete but we still need data, so request next without + // burst. + request_next_rest(work, item); } else { // There might be more coming, just wait for now. @@ -578,13 +585,88 @@ bool MavlinkFtpClient::download_burst_continue( return true; } + } else if (payload->req_opcode == CMD_READ_FILE) { + if (_debugging) { + LogWarn() << "Burst download continue missing pieces, write at " << payload->offset + << " for " << std::to_string(payload->size); + } + + item.ofstream.seekp(payload->offset); + if (item.ofstream.fail()) { + LogWarn() << "Seek failed"; + item.callback(ClientResult::FileIoError, {}); + return false; + } + + item.ofstream.write(reinterpret_cast(payload->data), payload->size); + if (!item.ofstream) { + item.callback(ClientResult::FileIoError, {}); + return false; + } + + // Keep track of what was written. + for (size_t i = payload->offset; i < payload->offset + payload->size; ++i) { + item.transferred[i] = DownloadBurstItem::Transferred::Yes; + } + + const size_t bytes_transferred = burst_bytes_transferred(item); + + if (_debugging) { + LogDebug() << "Written " << bytes_transferred << " of " << item.transferred.size() + << " bytes"; + } + + if (bytes_transferred == item.transferred.size()) { + // Final step + work.last_opcode = CMD_TERMINATE_SESSION; + + work.payload = {}; + work.payload.seq_number = work.last_sent_seq_number++; + work.payload.session = _session; + + work.payload.opcode = work.last_opcode; + work.payload.offset = 0; + work.payload.size = 0; + + start_timer(); + send_mavlink_ftp_message(work.payload); + return true; + } else { + item.callback( + ClientResult::Next, + ProgressData{ + static_cast(bytes_transferred), + static_cast(item.transferred.size())}); + + request_next_rest(work, item); + return true; + } + } else { LogErr() << "Unexpected req_opcode"; return false; } } -void MavlinkFtpClient::request_next_burst(Work& work, DownloadBurstItem& item) +void MavlinkFtpClient::request_burst(Work& work, DownloadBurstItem& item) +{ + UNUSED(item); + + work.last_opcode = CMD_BURST_READ_FILE; + work.payload = {}; + work.payload.seq_number = work.last_sent_seq_number++; + work.payload.session = _session; + work.payload.opcode = work.last_opcode; + work.payload.offset = 0; + + // Fill up the whole packet. + work.payload.size = max_data_length; + + start_timer(); + send_mavlink_ftp_message(work.payload); +} + +void MavlinkFtpClient::request_next_rest(Work& work, DownloadBurstItem& item) { const auto first_missing = std::find( item.transferred.begin(), item.transferred.end(), DownloadBurstItem::Transferred::No); @@ -597,6 +679,7 @@ void MavlinkFtpClient::request_next_burst(Work& work, DownloadBurstItem& item) std::find(first_missing, item.transferred.end(), DownloadBurstItem::Transferred::Yes); const size_t offset = std::distance(item.transferred.begin(), first_missing); + const uint32_t size = static_cast(std::distance(first_missing, last_missing_plus_one)); @@ -605,20 +688,30 @@ void MavlinkFtpClient::request_next_burst(Work& work, DownloadBurstItem& item) return; } - work.last_opcode = CMD_BURST_READ_FILE; + if (_debugging) { + LogDebug() << "Re-requesting from " << offset << " with size " << size; + } + + work.last_opcode = CMD_READ_FILE; work.payload = {}; work.payload.seq_number = work.last_sent_seq_number++; work.payload.session = _session; work.payload.opcode = work.last_opcode; work.payload.offset = offset; - work.payload.size = 4; - std::memcpy(&work.payload.data, &size, 4); + work.payload.size = + static_cast(std::min(static_cast(max_data_length), size)); start_timer(); send_mavlink_ftp_message(work.payload); } +size_t MavlinkFtpClient::burst_bytes_transferred(DownloadBurstItem& item) +{ + return std::count( + item.transferred.begin(), item.transferred.end(), DownloadBurstItem::Transferred::Yes); +} + bool MavlinkFtpClient::upload_start(Work& work, UploadItem& item) { std::error_code ec; @@ -1117,8 +1210,15 @@ void MavlinkFtpClient::timeout() LogDebug() << "Retries left: " << work->retries; } - start_timer(); - send_mavlink_ftp_message(work->payload); + { + const size_t bytes_transferred = burst_bytes_transferred(item); + if (bytes_transferred == 0 || bytes_transferred == item.transferred.size()) { + start_timer(); + send_mavlink_ftp_message(work->payload); + } else { + request_next_rest(*work, item); + } + } }, [&](UploadItem& item) { if (--work->retries == 0) { diff --git a/src/mavsdk/core/mavlink_ftp_client.h b/src/mavsdk/core/mavlink_ftp_client.h index 0179c2234a..53fcafd738 100644 --- a/src/mavsdk/core/mavlink_ftp_client.h +++ b/src/mavsdk/core/mavlink_ftp_client.h @@ -268,7 +268,9 @@ class MavlinkFtpClient { bool download_burst_start(Work& work, DownloadBurstItem& item); bool download_burst_continue(Work& work, DownloadBurstItem& item, PayloadHeader* payload); - void request_next_burst(Work& work, DownloadBurstItem& item); + void request_burst(Work& work, DownloadBurstItem& item); + void request_next_rest(Work& work, DownloadBurstItem& item); + size_t burst_bytes_transferred(DownloadBurstItem& item); bool upload_start(Work& work, UploadItem& item); bool upload_continue(Work& work, UploadItem& item); diff --git a/src/mavsdk/core/mavlink_ftp_server.cpp b/src/mavsdk/core/mavlink_ftp_server.cpp index fbb41fe75a..df9013de01 100644 --- a/src/mavsdk/core/mavlink_ftp_server.cpp +++ b/src/mavsdk/core/mavlink_ftp_server.cpp @@ -707,6 +707,10 @@ void MavlinkFtpServer::_work_read(const PayloadHeader& payload) return; } + if (_debugging) { + LogWarn() << "Read at " << payload.offset << " for " << int(payload.size); + } + _session_info.ifstream.read(reinterpret_cast(response.data), payload.size); if (_session_info.ifstream.fail()) { @@ -720,6 +724,7 @@ void MavlinkFtpServer::_work_read(const PayloadHeader& payload) const uint32_t bytes_read = _session_info.ifstream.gcount(); + response.offset = payload.offset; response.size = bytes_read; response.opcode = Opcode::RSP_ACK; @@ -764,20 +769,7 @@ void MavlinkFtpServer::_work_burst(const PayloadHeader& payload) } _session_info.burst_offset = payload.offset; - - if (payload.size != 4) { - response.seq_number = payload.seq_number + 1; - response.opcode = Opcode::RSP_NAK; - response.size = 1; - response.data[0] = ServerResult::ERR_INVALID_DATA_SIZE; - LogErr() << "Burst size invalid"; - _send_mavlink_ftp_message(response); - return; - } - - uint32_t burst_size; - std::memcpy(&burst_size, &payload.data, payload.size); - _session_info.burst_end = _session_info.burst_offset + burst_size; + _session_info.burst_chunk_size = payload.size; _burst_seq = payload.seq_number + 1; @@ -815,8 +807,8 @@ void MavlinkFtpServer::_send_burst_packet() void MavlinkFtpServer::_make_burst_packet(PayloadHeader& packet) { uint32_t bytes_to_read = std::min( - static_cast(max_data_length), - _session_info.burst_end - _session_info.burst_offset); + static_cast(_session_info.burst_chunk_size), + _session_info.file_size - _session_info.burst_offset); if (_debugging) { LogDebug() << "Burst read of " << bytes_to_read << " bytes"; @@ -838,7 +830,7 @@ void MavlinkFtpServer::_make_burst_packet(PayloadHeader& packet) packet.offset = _session_info.burst_offset; _session_info.burst_offset += bytes_read; - if (_session_info.burst_offset == _session_info.burst_end) { + if (_session_info.burst_offset == _session_info.file_size) { // Last read, we are done for this burst. packet.burst_complete = 1; if (_debugging) { diff --git a/src/mavsdk/core/mavlink_ftp_server.h b/src/mavsdk/core/mavlink_ftp_server.h index ba917d533e..dca1eac021 100644 --- a/src/mavsdk/core/mavlink_ftp_server.h +++ b/src/mavsdk/core/mavlink_ftp_server.h @@ -141,7 +141,7 @@ class MavlinkFtpServer { struct SessionInfo { uint32_t file_size{0}; uint32_t burst_offset{0}; - uint32_t burst_end{0}; + uint8_t burst_chunk_size{0}; std::ifstream ifstream; std::ofstream ofstream; } _session_info{};