From 5c9ae33151d62cd935af8fb521c8cd666a65310f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Mar 2020 15:10:11 +1100 Subject: [PATCH] GCS_MAVLink: support variable sized packets in burst read this allows packet size to be tailored to the transport --- libraries/GCS_MAVLink/GCS_FTP.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 121e773c95..0ed02d8108 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -437,6 +437,7 @@ void GCS_MAVLINK::ftp_worker(void) { } case FTP_OP::BurstReadFile: { + const uint16_t max_read = (request.size == 0?sizeof(reply.data):request.size); // must actually be working on a file if (ftp.fd == -1) { ftp_error(reply, FTP_ERROR::FileNotFound); @@ -459,7 +460,7 @@ void GCS_MAVLINK::ftp_worker(void) { const uint32_t transfer_size = 100; for (uint32_t i = 0; (i < transfer_size) && more_pending; i++) { // fill the buffer - const ssize_t read_bytes = AP::FS().read(ftp.fd, reply.data, sizeof(reply.data)); + const ssize_t read_bytes = AP::FS().read(ftp.fd, reply.data, max_read); if (read_bytes == -1) { ftp_error(reply, FTP_ERROR::FailErrno); more_pending = false; @@ -478,7 +479,7 @@ void GCS_MAVLINK::ftp_worker(void) { } reply.opcode = FTP_OP::Ack; - reply.offset = request.offset + i * sizeof(reply.data); + reply.offset = request.offset + i * max_read; reply.burst_complete = (i == (transfer_size - 1)); reply.size = (uint8_t)read_bytes;