From e3de88d1d476b9989b0ecca504f4face0c1c968c Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 18 Feb 2021 21:32:08 +1100
Subject: [PATCH] GCS_MAVLink: handle MAV_CMD_DO_SET_MISSION_CURRENT

---
 libraries/GCS_MAVLink/GCS.h          |  8 +++++++
 libraries/GCS_MAVLink/GCS_Common.cpp | 34 ++++++++++++++++++++++++++++
 2 files changed, 42 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index d9f5383a3a..0ce913f6e1 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -389,6 +389,12 @@ protected:
     void handle_mission_request(const mavlink_message_t &msg) const;
     void handle_mission_request_int(const mavlink_message_t &msg) const;
     void handle_mission_clear_all(const mavlink_message_t &msg) const;
+
+    // Note that there exists a relatively new mavlink DO command,
+    // MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
+    // that the command has been received, rather than the GCS having to
+    // rely on getting back an identical sequence number as some currently
+    // do.
     virtual void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg);
     void handle_mission_count(const mavlink_message_t &msg);
     void handle_mission_write_partial_list(const mavlink_message_t &msg);
@@ -468,6 +474,8 @@ protected:
 
     MAV_RESULT handle_command_preflight_can(const mavlink_command_long_t &packet);
 
+    virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet);
+
     MAV_RESULT handle_command_battery_reset(const mavlink_command_long_t &packet);
     void handle_command_long(const mavlink_message_t &msg);
     MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 1c3ba7d941..3fc5b3604a 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -516,6 +516,12 @@ void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg) const
 
 /*
   handle a MISSION_SET_CURRENT mavlink packet
+
+  Note that there exists a relatively new mavlink DO command,
+  MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
+  that the command has been received, rather than the GCS having to
+  rely on getting back an identical sequence number as some currently
+  do.
  */
 void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg)
 {
@@ -3788,6 +3794,29 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_
 #endif
 }
 
+// changes the current waypoint; at time of writing GCS
+// implementations use the mavlink message MISSION_SET_CURRENT to set
+// the current waypoint, rather than this DO command.  It is hoped we
+// can move to this command in the future to avoid acknowledgement
+// issues with MISSION_SET_CURRENT
+MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_command_long_t &packet)
+{
+    AP_Mission *mission = AP::mission();
+    if (mission == nullptr) {
+        return MAV_RESULT_UNSUPPORTED;
+    }
+
+    const uint32_t seq = (uint32_t)packet.param1;
+    if (!mission->set_current_cmd(seq)) {
+        return MAV_RESULT_FAILED;
+    }
+
+    // volunteer the new current waypoint for all listeners
+    send_message(MSG_CURRENT_WAYPOINT);
+
+    return MAV_RESULT_ACCEPTED;
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_battery_reset(const mavlink_command_long_t &packet)
 {
     const uint16_t battery_mask = packet.param1;
@@ -4059,6 +4088,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
         result = handle_command_preflight_calibration(packet);
         break;
 
+
+    case MAV_CMD_DO_SET_MISSION_CURRENT:
+        result = handle_command_do_set_mission_current(packet);
+        break;
+
     case MAV_CMD_BATTERY_RESET:
         result = handle_command_battery_reset(packet);
         break;