From 3f7f7cd3a11d00aa5e5315b0f98db7a21f4846a8 Mon Sep 17 00:00:00 2001
From: Henry Wurzburg <hwurzburg@yahoo.com>
Date: Mon, 19 Jun 2023 10:48:59 -0500
Subject: [PATCH] Plane:remove duplication in setting servos in MANUAL

---
 ArduPlane/Plane.h         |  1 -
 ArduPlane/mode_manual.cpp | 19 +++++++++++++++++++
 ArduPlane/quadplane.h     |  1 +
 ArduPlane/servos.cpp      | 30 +-----------------------------
 4 files changed, 21 insertions(+), 30 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 4a5dd56030..c7eeab8710 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1089,7 +1089,6 @@ private:
     // servos.cpp
     void set_servos_idle(void);
     void set_servos();
-    void set_servos_manual_passthrough(void);
     void set_servos_controlled(void);
     void set_servos_old_elevons(void);
     void set_servos_flaps(void);
diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp
index d04767f377..4dc3d067d8 100644
--- a/ArduPlane/mode_manual.cpp
+++ b/ArduPlane/mode_manual.cpp
@@ -5,7 +5,26 @@ void ModeManual::update()
 {
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false));
     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false));
+    //rudder in sets rudder, but is also assigned to steering values used later in servos.cpp for steering
     plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.steering_control.steering);
+    float throttle = plane.get_throttle_input(true);
+
+
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) {
+        // for quadplanes it can be useful to run the idle governor in MANUAL mode
+        // as it prevents the VTOL motors from running
+        int8_t min_throttle = plane.aparm.throttle_min.get();
+
+        // apply idle governor
+#if AP_ICENGINE_ENABLED
+        plane.g2.ice_control.update_idle_governor(min_throttle);
+#endif
+        throttle = MAX(throttle, min_throttle);
+    }
+#endif
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
 
     plane.nav_roll_cd = ahrs.roll_sensor;
     plane.nav_pitch_cd = ahrs.pitch_sensor;
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 887f3bc9cf..3575186aaa 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -47,6 +47,7 @@ public:
     friend class Tailsitter_Transition;
 
     friend class Mode;
+    friend class ModeManual;
     friend class ModeAuto;
     friend class ModeRTL;
     friend class ModeAvoidADSB;
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index 1b3c7f6a30..a1ee77c5d0 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -376,32 +376,6 @@ void Plane::set_servos_idle(void)
     SRV_Channels::output_ch_all();
 }
 
-/*
-  pass through channels in manual mode
- */
-void Plane::set_servos_manual_passthrough(void)
-{
-    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_in_expo(false));
-    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_in_expo(false));
-    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_in_expo(false));
-    float throttle = get_throttle_input(true);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
-
-#if HAL_QUADPLANE_ENABLED
-    if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) {
-        // for quadplanes it can be useful to run the idle governor in MANUAL mode
-        // as it prevents the VTOL motors from running
-        int8_t min_throttle = aparm.throttle_min.get();
-
-        // apply idle governor
-#if AP_ICENGINE_ENABLED
-        g2.ice_control.update_idle_governor(min_throttle);
-#endif
-        throttle = MAX(throttle, min_throttle);
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
-    }
-#endif
-}
 
 /*
   Scale the throttle to conpensate for battery voltage drop
@@ -870,9 +844,7 @@ void Plane::set_servos(void)
     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
     SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering);
 
-    if (control_mode == &mode_manual) {
-        set_servos_manual_passthrough();
-    } else {
+    if (control_mode != &mode_manual) {
         set_servos_controlled();
     }