From c6e7ba81312e7b6410882a22a24be68abe5e2dad Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 23 Jan 2024 15:10:33 +1100
Subject: [PATCH] AP_FileSystem: permit upload of fence/rally via ftp

---
 .../AP_Filesystem/AP_Filesystem_Mission.cpp   | 129 +++++++++++++++++-
 .../AP_Filesystem/AP_Filesystem_Mission.h     |   3 +
 2 files changed, 125 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp
index 59103f3f8a..cba03addcd 100644
--- a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp
+++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp
@@ -27,6 +27,7 @@
 #include <AP_Rally/AP_Rally.h>
 #include <GCS_MAVLink/MissionItemProtocol_Rally.h>
 #include <GCS_MAVLink/MissionItemProtocol_Fence.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 extern int errno;
@@ -211,10 +212,12 @@ int AP_Filesystem_Mission::stat(const char *name, struct stat *stbuf)
  */
 bool AP_Filesystem_Mission::check_file_name(const char *name, enum MAV_MISSION_TYPE &mtype)
 {
+#if AP_MISSION_ENABLED
     if (strcmp(name, "mission.dat") == 0) {
         mtype = MAV_MISSION_TYPE_MISSION;
         return true;
     }
+#endif
 #if AP_FENCE_ENABLED
     if (strcmp(name, "fence.dat") == 0) {
         mtype = MAV_MISSION_TYPE_FENCE;
@@ -261,6 +264,7 @@ bool AP_Filesystem_Mission::get_item(uint32_t idx, enum MAV_MISSION_TYPE mtype,
 uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const
 {
     switch (mtype) {
+#if AP_MISSION_ENABLED
     case MAV_MISSION_TYPE_MISSION: {
         auto *mission = AP::mission();
         if (!mission) {
@@ -268,18 +272,17 @@ uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const
         }
         return mission->num_commands();
     }
-        
-    case MAV_MISSION_TYPE_FENCE: {
+#endif
+
 #if AP_FENCE_ENABLED
+    case MAV_MISSION_TYPE_FENCE: {
         auto *fence = AP::fence();
         if (fence == nullptr) {
             return 0;
         }
         return fence->polyfence().num_stored_items();
-#else
-        return 0;
-#endif
     }
+#endif
 
 #if HAL_RALLY_ENABLED
     case MAV_MISSION_TYPE_RALLY: {
@@ -381,6 +384,30 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r)
         }
     }
 
+    switch (r.mtype) {
+#if AP_MISSION_ENABLED
+    case MAV_MISSION_TYPE_MISSION:
+        return finish_upload_mission(hdr, r, b);
+#endif
+#if HAL_RALLY_ENABLED
+    case MAV_MISSION_TYPE_RALLY:
+        return finish_upload_rally(hdr, r, b);
+#endif
+#if AP_FENCE_ENABLED
+    case MAV_MISSION_TYPE_FENCE:
+        return finish_upload_fence(hdr, r, b);
+#endif
+    default:
+        // really should not get here....
+        break;
+    }
+
+    return false;
+}
+
+#if AP_MISSION_ENABLED
+bool AP_Filesystem_Mission::finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b)
+{
     auto *mission = AP::mission();
     if (mission == nullptr) {
         return false;
@@ -389,16 +416,17 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r)
     if ((hdr.options & unsigned(Options::NO_CLEAR)) == 0) {
         mission->clear();
     }
-    for (uint32_t i=0; i<nitems; i++) {
+    for (uint32_t i=0; i<hdr.num_items; i++) {
         mavlink_mission_item_int_t m {};
         AP_Mission::Mission_Command cmd;
+        const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
         memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
         const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(m, cmd);
         if (res != MAV_MISSION_ACCEPTED) {
             return false;
         }
         if (cmd.id == MAV_CMD_DO_JUMP &&
-            (cmd.content.jump.target >= nitems || cmd.content.jump.target == 0)) {
+            (cmd.content.jump.target >= hdr.num_items || cmd.content.jump.target == 0)) {
             return false;
         }
         uint16_t idx = i + hdr.start;
@@ -414,5 +442,92 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r)
     }
     return true;
 }
+#endif  // AP_MISSION_ENABLED
+
+#if AP_FENCE_ENABLED
+bool AP_Filesystem_Mission::finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b)
+{
+    bool success = false;
+
+    AC_PolyFenceItem *new_items = nullptr;
+
+    auto *fence = AP::fence();
+    if (fence == nullptr) {
+        goto OUT;
+    }
+
+    if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) {
+        // Only complete fences can be uploaded for now.
+        goto OUT;
+    }
+
+    // passing nullptr and 0 items through to Polyfence loader is
+    // absolutely OK:
+    if (hdr.num_items != 0) {
+        new_items = new AC_PolyFenceItem[hdr.num_items];
+        if (new_items == nullptr) {
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Out of memory for upload");
+            goto OUT;
+        }
+    }
+
+    // convert from MISSION_ITEM_INT to AC_PolyFenceItem:
+    for (uint32_t i=0; i<hdr.num_items; i++) {
+        mavlink_mission_item_int_t m {};
+        const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
+        memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
+        const MAV_MISSION_RESULT res = MissionItemProtocol_Fence::convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(m, new_items[i]);
+        if (res != MAV_MISSION_ACCEPTED) {
+            goto OUT;
+        }
+    }
+
+    success = fence->polyfence().write_fence(new_items, hdr.num_items);
+
+OUT:
+
+    delete[] new_items;
+
+    return success;
+}
+#endif  // AP_FENCE_ENABLED
+
+#if HAL_RALLY_ENABLED
+bool AP_Filesystem_Mission::finish_upload_rally(const struct header &hdr, const rfile &r, const uint8_t *b)
+{
+    bool success = false;
+
+    auto *rally = AP::rally();
+    if (rally == nullptr) {
+        goto OUT;
+    }
+
+    if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) {
+        //only complete sets of rally points can be added ATM
+        goto OUT;
+    }
+
+    rally->truncate(0);
+
+    for (uint32_t i=0; i<hdr.num_items; i++) {
+        mavlink_mission_item_int_t m {};
+        RallyLocation cmd;
+        const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
+        memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
+        const MAV_MISSION_RESULT res = MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(m, cmd);
+        if (res != MAV_MISSION_ACCEPTED) {
+            goto OUT;
+        }
+
+        if (!rally->append(cmd)) {
+            goto OUT;
+        }
+    }
+    success = true;
+
+OUT:
+    return success;
+}
+#endif  // HAL_RALLY_ENABLED
 
 #endif  // AP_FILESYSTEM_MISSION_ENABLED
diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.h b/libraries/AP_Filesystem/AP_Filesystem_Mission.h
index 5a6e123527..88a97781e9 100644
--- a/libraries/AP_Filesystem/AP_Filesystem_Mission.h
+++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.h
@@ -72,6 +72,9 @@ private:
 
     // finish loading items
     bool finish_upload(const rfile &r);
+    bool finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b);
+    bool finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b);
+    bool finish_upload_rally(const struct header &hdr, const rfile &r, const uint8_t *b);
 
     // see if a block of memory is all zero
     bool all_zero(const uint8_t *b, uint8_t size) const;