From 2f42f55278b840fb92c16b16c73925da8ac73f80 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Wed, 31 Jan 2024 15:16:30 +0000
Subject: [PATCH] Plane: move pilot throttle output up to control modes

---
 ArduPlane/mode.cpp           | 15 +++++++++++++++
 ArduPlane/mode.h             |  7 +++++++
 ArduPlane/mode_acro.cpp      |  2 ++
 ArduPlane/mode_autotune.cpp  |  7 +++++++
 ArduPlane/mode_fbwa.cpp      |  8 ++++++++
 ArduPlane/mode_stabilize.cpp |  2 ++
 ArduPlane/mode_training.cpp  |  2 ++
 ArduPlane/servos.cpp         | 17 +----------------
 8 files changed, 44 insertions(+), 16 deletions(-)

diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp
index ec10b69631..30ec5fc5b0 100644
--- a/ArduPlane/mode.cpp
+++ b/ArduPlane/mode.cpp
@@ -259,6 +259,21 @@ void Mode::output_rudder_and_steering(float val)
     SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val);
 }
 
+// Output pilot throttle, this is used in stabilized modes without auto throttle control
+// Direct mapping if THR_PASS_STAB is set
+// Otherwise apply curve for trim correction if configured
+void Mode::output_pilot_throttle()
+{
+    if (plane.g.throttle_passthru_stabilize) {
+        // THR_PASS_STAB set, direct mapping
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
+        return;
+    }
+
+    // get throttle, but adjust center to output TRIM_THROTTLE if flight option set
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true));
+}
+
 // true if throttle min/max limits should be applied
 bool Mode::use_throttle_limits() const
 {
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 2c10d2a94e..b7f4732e92 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -155,6 +155,9 @@ protected:
     // Helper to output to both k_rudder and k_steering servo functions
     void output_rudder_and_steering(float val);
 
+    // Output pilot throttle, this is used in stabilized modes without auto throttle control
+    void output_pilot_throttle();
+
 #if HAL_QUADPLANE_ENABLED
     // References for convenience, used by QModes
     AC_PosControl*& pos_control;
@@ -262,6 +265,8 @@ public:
     
     bool mode_allows_autotuning() const override { return true; }
 
+    void run() override;
+
 protected:
 
     bool _enter() override;
@@ -493,6 +498,8 @@ public:
     
     bool mode_allows_autotuning() const override { return true; }
 
+    void run() override;
+
 };
 
 class ModeFBWB : public Mode
diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp
index c15970b9bd..7093fd96b8 100644
--- a/ArduPlane/mode_acro.cpp
+++ b/ArduPlane/mode_acro.cpp
@@ -26,6 +26,8 @@ void ModeAcro::update()
 
 void ModeAcro::run()
 {
+    output_pilot_throttle();
+
     if (plane.g.acro_locking == 2 && plane.g.acro_yaw_rate > 0 &&
         plane.yawController.rate_control_enabled()) {
         // we can do 3D acro locking
diff --git a/ArduPlane/mode_autotune.cpp b/ArduPlane/mode_autotune.cpp
index 395a6d6ef5..cbbf5a22bc 100644
--- a/ArduPlane/mode_autotune.cpp
+++ b/ArduPlane/mode_autotune.cpp
@@ -14,3 +14,10 @@ void ModeAutoTune::update()
     plane.mode_fbwa.update();
 }
 
+void ModeAutoTune::run()
+{
+    // Run base class function and then output throttle
+    Mode::run();
+
+    output_pilot_throttle();
+}
diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp
index 6fd5ea877f..ff53a835ee 100644
--- a/ArduPlane/mode_fbwa.cpp
+++ b/ArduPlane/mode_fbwa.cpp
@@ -35,3 +35,11 @@ void ModeFBWA::update()
         }
     }
 }
+
+void ModeFBWA::run()
+{
+    // Run base class function and then output throttle
+    Mode::run();
+
+    output_pilot_throttle();
+}
diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp
index b73dfb6fc2..ee0f7626de 100644
--- a/ArduPlane/mode_stabilize.cpp
+++ b/ArduPlane/mode_stabilize.cpp
@@ -13,4 +13,6 @@ void ModeStabilize::run()
     plane.stabilize_pitch();
     stabilize_stick_mixing_direct();
     plane.stabilize_yaw();
+
+    output_pilot_throttle();
 }
diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp
index 1c86e6d9c9..eb7040ce6f 100644
--- a/ArduPlane/mode_training.cpp
+++ b/ArduPlane/mode_training.cpp
@@ -66,4 +66,6 @@ void ModeTraining::run()
     // Always manual rudder control
     output_rudder_and_steering(plane.rudder_in_expo(false));
 
+    output_pilot_throttle();
+
 }
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index 8e91fd20de..c0cab9f9de 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -585,23 +585,8 @@ void Plane::set_throttle(void)
 #if AP_SCRIPTING_ENABLED
     if (nav_scripting_active()) {
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
-    } else
-#endif
-           if (control_mode == &mode_stabilize ||
-               control_mode == &mode_training ||
-               control_mode == &mode_acro ||
-               control_mode == &mode_fbwa ||
-               control_mode == &mode_autotune) {
-        // a manual throttle mode
-        if (g.throttle_passthru_stabilize) {
-            // manual pass through of throttle while in FBWA or
-            // STABILIZE mode with THR_PASS_STAB set
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
-        } else {
-            // get throttle, but adjust center to output TRIM_THROTTLE if flight option set
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_adjusted_throttle_input(true));
-        }
     }
+#endif
 
     if (control_mode->use_battery_compensation()) {
         // Apply voltage compensation to throttle output from flight mode