From 5bc2b1a09b9b1b1b0dd81f147d6d65c96c9f1049 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 24 Jan 2024 09:58:17 +1100
Subject: [PATCH] Plane: break out a method for set-position-target-global-int

---
 ArduPlane/GCS_Mavlink.cpp | 20 +++++++++++---------
 ArduPlane/GCS_Mavlink.h   |  2 ++
 2 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index c57d3e22f4..c0e1a36868 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -1329,6 +1329,16 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
     }
 
     case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
+        handle_set_position_target_global_int(msg);
+        break;
+
+    default:
+        GCS_MAVLINK::handle_message(msg);
+        break;
+    } // end switch
+} // end handle mavlink
+
+void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_message_t &msg)
     {
         // Only want to allow companion computer position control when
         // in a certain mode to avoid inadvertently sending these
@@ -1338,7 +1348,7 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
         // one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
         if (plane.control_mode != &plane.mode_guided) {
             //don't screw up failsafes
-            break;
+            return;
         }
 
         mavlink_set_position_target_global_int_t pos_target;
@@ -1378,16 +1388,8 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
                 handle_change_alt_request(cmd);
             }
         } // end if alt_mask       
-
-        break;
     }
 
-    default:
-        GCS_MAVLINK::handle_message(msg);
-        break;
-    } // end switch
-} // end handle mavlink
-
 MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)
 {
     const MAV_RESULT result = GCS_MAVLINK::handle_command_do_set_mission_current(packet);
diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h
index 7543dea8c3..3f737838b6 100644
--- a/ArduPlane/GCS_Mavlink.h
+++ b/ArduPlane/GCS_Mavlink.h
@@ -66,6 +66,8 @@ private:
     MAV_RESULT handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet);
     MAV_RESULT handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet);
 
+    void handle_set_position_target_global_int(const mavlink_message_t &msg);
+
 #if HAL_QUADPLANE_ENABLED
 #if AP_MAVLINK_COMMAND_LONG_ENABLED
     void convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out);