Plane: move pilot throttle output up to control modes

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
 {

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

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

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();
+}

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();
+}

ArduPlane/mode_stabilize.cpp

@@ -13,4 +13,6 @@ void ModeStabilize::run()
     plane.stabilize_pitch();
     stabilize_stick_mixing_direct();
     plane.stabilize_yaw();
+
+    output_pilot_throttle();
 }

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();
+
 }

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));
-        }
-    }
 
     if (control_mode->use_battery_compensation()) {
         // Apply voltage compensation to throttle output from flight mode