From c5a3dfaa575e6adfe8595fa8e5758dca343c4945 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 22 Nov 2022 14:33:22 +1100
Subject: [PATCH] ArduCopter: factor out handle_manual_control_axes

---
 ArduCopter/GCS_Mavlink.cpp | 45 +++++++++++++-------------------------
 ArduCopter/GCS_Mavlink.h   |  2 ++
 2 files changed, 17 insertions(+), 30 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 1b1a39476c..a20beb419c 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -1055,6 +1055,21 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
 }
 #endif
 
+// this is called on receipt of a MANUAL_CONTROL packet and is
+// expected to call manual_override to override RC input on desired
+// axes.
+void GCS_MAVLINK_Copter::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow)
+{
+    if (packet.z < 0) { // Copter doesn't do negative thrust
+        return;
+    }
+
+    manual_override(copter.channel_roll, packet.y, 1000, 2000, tnow);
+    manual_override(copter.channel_pitch, packet.x, 1000, 2000, tnow, true);
+    manual_override(copter.channel_throttle, packet.z, 0, 1000, tnow);
+    manual_override(copter.channel_yaw, packet.r, 1000, 2000, tnow);
+}
+
 void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
 {
     // for mavlink SET_POSITION_TARGET messages
@@ -1082,36 +1097,6 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
 
     switch (msg.msgid) {
 
-    case MAVLINK_MSG_ID_MANUAL_CONTROL:
-    {
-        if (msg.sysid != copter.g.sysid_my_gcs) {
-            break; // only accept control from our gcs
-        }
-
-        mavlink_manual_control_t packet;
-        mavlink_msg_manual_control_decode(&msg, &packet);
-
-        if (packet.target != copter.g.sysid_this_mav) {
-            break; // only accept control aimed at us
-        }
-
-        if (packet.z < 0) { // Copter doesn't do negative thrust
-            break;
-        }
-
-        uint32_t tnow = AP_HAL::millis();
-
-        manual_override(copter.channel_roll, packet.y, 1000, 2000, tnow);
-        manual_override(copter.channel_pitch, packet.x, 1000, 2000, tnow, true);
-        manual_override(copter.channel_throttle, packet.z, 0, 1000, tnow);
-        manual_override(copter.channel_yaw, packet.r, 1000, 2000, tnow);
-
-        // a manual control message is considered to be a 'heartbeat'
-        // from the ground station for failsafe purposes
-        gcs().sysid_myggcs_seen(tnow);
-        break;
-    }
-
 #if MODE_GUIDED_ENABLED == ENABLED
     case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
     {
diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h
index a68096a9f5..11e1fc4a62 100644
--- a/ArduCopter/GCS_Mavlink.h
+++ b/ArduCopter/GCS_Mavlink.h
@@ -51,6 +51,8 @@ protected:
     virtual MAV_VTOL_STATE vtol_state() const override { return MAV_VTOL_STATE_MC; };
     virtual MAV_LANDED_STATE landed_state() const override;
 
+    void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override;
+
 private:
 
     void handleMessage(const mavlink_message_t &msg) override;