From 8d18b973ff9ee9af164cadbc1e99a706eac9200d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 2 Nov 2019 21:15:51 +1100
Subject: [PATCH] GCS_MAVLink: added ftp_push_replies()

---
 libraries/GCS_MAVLink/GCS.h       |  1 +
 libraries/GCS_MAVLink/GCS_FTP.cpp | 25 ++++++++++++-------------
 2 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 4927b7c482..1063962140 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -688,6 +688,7 @@ private:
     void handle_file_transfer_protocol(const mavlink_message_t &msg);
     void send_ftp_replies(void);
     void ftp_worker(void);
+    void ftp_push_replies(pending_ftp &reply);
 #endif // HAVE_FILESYSTEM_SUPPORT
 
     void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const;
diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp
index 57e53deb0a..6f1aae2a4a 100644
--- a/libraries/GCS_MAVLink/GCS_FTP.cpp
+++ b/libraries/GCS_MAVLink/GCS_FTP.cpp
@@ -143,6 +143,14 @@ 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)
+{
+    while (!ftp.replies->push(reply)) { // we must fit the response, keep shoving it in
+        hal.scheduler->delay(10);
+    }
+}
+
 void GCS_MAVLINK::ftp_worker(void) {
     pending_ftp request;
     pending_ftp reply = {};
@@ -157,9 +165,7 @@ void GCS_MAVLINK::ftp_worker(void) {
         // if it's a rerequest and we still have the last response then send it
         if ((request.sysid == reply.sysid) && (request.compid = reply.compid) &&
             (request.session == reply.session) && (request.seq_number + 1 == reply.seq_number)) {
-            while (!ftp.replies->push(reply)) {
-                hal.scheduler->delay(10);
-            }
+            ftp_push_replies(reply);
             continue;
         }
 
@@ -175,9 +181,7 @@ void GCS_MAVLINK::ftp_worker(void) {
         // sanity check the request size
         if (request.size > sizeof(request.data)) {
             ftp_error(reply, FTP_ERROR::InvalidDataSize);
-            while (!ftp.replies->push(reply)) {
-                hal.scheduler->delay(10);
-            }
+            ftp_push_replies(reply);
             continue;
         }
 
@@ -477,9 +481,7 @@ void GCS_MAVLINK::ftp_worker(void) {
                             reply.burst_complete = (i == (transfer_size - 1));
                             reply.size = (uint8_t)read_bytes;
 
-                            while (!ftp.replies->push(reply)) { // we must fit the response, keep shoving it in
-                                hal.scheduler->delay(10);
-                            }
+                            ftp_push_replies(reply);
 
                             // prep the reply to be used again
                             reply.seq_number++;
@@ -497,10 +499,7 @@ void GCS_MAVLINK::ftp_worker(void) {
             }
         }
 
-        // send our response back out to the system
-        while (!ftp.replies->push(reply)) { // we must fit the response, keep shoving it in
-            hal.scheduler->delay(10);
-        }
+        ftp_push_replies(reply);
         continue;
     }
 }