GCS_MAVLink: factor out a handle_mission_guided_mode_request method

ArduPilot · Oct 29, 2024 · e8c6093 · e8c6093
1 parent a6f00a3
commit e8c6093
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 22 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -1126,6 +1126,10 @@ class GCS_MAVLINK
     // true if we should NOT do MAVLink on this port (usually because
     // someone's doing SERIAL_CONTROL over mavlink)
     bool _locked;
+
+    // handle a mission item int uploaded with current==2 or
+    // current==3, meaning go somewhere when in guided mode:
+    void handle_mission_item_guided_mode_request(const mavlink_message_t &msg, const mavlink_mission_item_int_t &mission_item_int);
 };
 
 /// @class GCS

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -912,6 +912,36 @@ void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg)
 #endif
 }
 
+#if AP_MISSION_ENABLED
+void GCS_MAVLINK::handle_mission_item_guided_mode_request(const mavlink_message_t &msg, const mavlink_mission_item_int_t &mission_item_int)
+{
+        const uint8_t current = mission_item_int.current;
+
+        struct AP_Mission::Mission_Command cmd = {};
+        MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
+        if (result != MAV_MISSION_ACCEPTED) {
+            //decode failed
+            send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
+            return;
+        }
+        // guided or change-alt
+        if (current == 2) {
+            // current = 2 is a flag to tell us this is a "guided mode"
+            // waypoint and not for the mission
+            result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
+                      : MAV_MISSION_ERROR) ;
+        } else if (current == 3) {
+            //current = 3 is a flag to tell us this is a alt change only
+            // add home alt if needed
+            handle_change_alt_request(cmd);
+
+            // verify we received the command
+            result = MAV_MISSION_ACCEPTED;
+        }
+        send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
+}
+#endif  // AP_MISSION_ENABLED
+
 void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
 {
     mavlink_mission_item_int_t mission_item_int;
@@ -945,28 +975,7 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
     const uint8_t current = mission_item_int.current;
 
     if (type == MAV_MISSION_TYPE_MISSION && (current == 2 || current == 3)) {
-        struct AP_Mission::Mission_Command cmd = {};
-        MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
-        if (result != MAV_MISSION_ACCEPTED) {
-            //decode failed
-            send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
-            return;
-        }
-        // guided or change-alt
-        if (current == 2) {
-            // current = 2 is a flag to tell us this is a "guided mode"
-            // waypoint and not for the mission
-            result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
-                      : MAV_MISSION_ERROR) ;
-        } else if (current == 3) {
-            //current = 3 is a flag to tell us this is a alt change only
-            // add home alt if needed
-            handle_change_alt_request(cmd);
-
-            // verify we received the command
-            result = MAV_MISSION_ACCEPTED;
-        }
-        send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
+        handle_mission_item_guided_mode_request(msg, mission_item_int);
         return;
     }
 #endif