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{};