From f96add13e9fef514ed4f2016630637cbe58d8e86 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 24 Jan 2024 10:07:19 +1100
Subject: [PATCH] Plane: break out a method for set-position-target-local-ned

---
 ArduPlane/GCS_Mavlink.cpp | 44 ++++++++++++++++++++-------------------
 ArduPlane/GCS_Mavlink.h   |  1 +
 2 files changed, 24 insertions(+), 21 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index c0e1a36868..ea7b291c59 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -1305,28 +1305,8 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
     }
 
     case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
-    {
-        // decode packet
-        mavlink_set_position_target_local_ned_t packet;
-        mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
-
-        // exit if vehicle is not in Guided mode
-        if (plane.control_mode != &plane.mode_guided) {
-            break;
-        }
-
-        // only local moves for now
-        if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
-            break;
-        }
-
-        // just do altitude for now
-        plane.next_WP_loc.alt += -packet.z*100.0;
-        gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
-                        (double)((plane.next_WP_loc.alt - plane.home.alt)*0.01));
-        
+        handle_set_position_target_local_ned(msg);
         break;
-    }
 
     case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
         handle_set_position_target_global_int(msg);
@@ -1338,6 +1318,28 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
     } // end switch
 } // end handle mavlink
 
+void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_message_t &msg)
+    {
+        // decode packet
+        mavlink_set_position_target_local_ned_t packet;
+        mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
+
+        // exit if vehicle is not in Guided mode
+        if (plane.control_mode != &plane.mode_guided) {
+            return;
+        }
+
+        // only local moves for now
+        if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
+            return;
+        }
+
+        // just do altitude for now
+        plane.next_WP_loc.alt += -packet.z*100.0;
+        gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
+                        (double)((plane.next_WP_loc.alt - plane.home.alt)*0.01));
+    }
+
 void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_message_t &msg)
     {
         // Only want to allow companion computer position control when
diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h
index 3f737838b6..e9df972aba 100644
--- a/ArduPlane/GCS_Mavlink.h
+++ b/ArduPlane/GCS_Mavlink.h
@@ -67,6 +67,7 @@ private:
     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);
+    void handle_set_position_target_local_ned(const mavlink_message_t &msg);
 
 #if HAL_QUADPLANE_ENABLED
 #if AP_MAVLINK_COMMAND_LONG_ENABLED