Plane: ask flight mode if min/max throttle limits should be applied

ArduPlane/Plane.h

@@ -1095,6 +1095,7 @@ class Plane : public AP_Vehicle {
     // servos.cpp
     void set_servos_idle(void);
     void set_servos();
+    float apply_throttle_limits(float throttle_in);
     void set_throttle(void);
     void set_takeoff_expected(void);
     void set_servos_old_elevons(void);

ArduPlane/mode.cpp

@@ -258,3 +258,35 @@ void Mode::output_rudder_and_steering(float val)
     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val);
     SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val);
 }
+
+// true if throttle min/max limits should be applied
+bool Mode::use_throttle_limits() const
+{
+#if AP_SCRIPTING_ENABLED
+    if (plane.nav_scripting_active()) {
+        return false;
+    }
+#endif
+
+    if (this == &plane.mode_stabilize ||
+        this == &plane.mode_training ||
+        this == &plane.mode_acro ||
+        this == &plane.mode_fbwa ||
+        this == &plane.mode_autotune) {
+        // a manual throttle mode
+        return !plane.g.throttle_passthru_stabilize;
+    }
+
+    if (is_guided_mode() && plane.guided_throttle_passthru) {
+        // manual pass through of throttle while in GUIDED
+        return false;
+    }
+
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.in_vtol_mode()) {
+        return quadplane.allow_forward_throttle_in_vtol_mode();
+    }
+#endif
+
+    return true;
+}

ArduPlane/mode.h

@@ -135,6 +135,8 @@ class Mode
     // true if is taking 
     virtual bool is_taking_off() const;
 
+    // true if throttle min/max limits should be applied
+    bool use_throttle_limits() const;
 
 protected:
 

ArduPlane/quadplane.cpp

@@ -4742,4 +4742,10 @@ float QuadPlane::get_throttle_input() const
     return ret;
 }
 
+// return true if forward throttle from forward_throttle_pct() should be used
+bool QuadPlane::allow_forward_throttle_in_vtol_mode() const
+{
+    return in_vtol_mode() && motors->armed() && (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN);
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/quadplane.h

@@ -729,6 +729,11 @@ class QuadPlane
      */
     void setup_rp_fw_angle_gains(void);
 
+    /*
+      return true if forward throttle from forward_throttle_pct() should be used
+     */
+    bool allow_forward_throttle_in_vtol_mode() const;
+
 public:
     void motor_test_output();
     MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,

ArduPlane/servos.cpp

@@ -495,11 +495,11 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
     }
 }
 #endif // #if AP_BATTERY_WATT_MAX_ENABLED
-    
+
 /*
-  setup output channels all non-manual modes
+  Apply min/max limits to throttle
  */
-void Plane::set_throttle(void)
+float Plane::apply_throttle_limits(float throttle_in)
 {
     // convert 0 to 100% (or -100 to +100) into PWM
     int8_t min_throttle = aparm.throttle_min.get();
@@ -531,14 +531,22 @@ void Plane::set_throttle(void)
     }
 
     // compensate for battery voltage drop
-    g2.fwd_batt_cmp.update();
     g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle);
 
 #if AP_BATTERY_WATT_MAX_ENABLED
     // apply watt limiter
     throttle_watt_limiter(min_throttle, max_throttle);
 #endif
 
+    return constrain_float(throttle_in, min_throttle, max_throttle);
+}
+
+/*
+  setup output channels all non-manual modes
+ */
+void Plane::set_throttle(void)
+{
+
     if (!arming.is_armed_and_safety_off()) {
         // Always set 0 scaled even if overriding to zero pwm.
         // This ensures slew limits and other functions using the scaled value pick up in the correct place
@@ -551,7 +559,10 @@ void Plane::set_throttle(void)
             SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
             SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
         }
-    } else if (suppress_throttle()) {
+        return;
+    }
+
+    if (suppress_throttle()) {
         if (g.throttle_suppress_manual) {
             // manual pass through of throttle while throttle is suppressed
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
@@ -565,11 +576,18 @@ void Plane::set_throttle(void)
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
 
         }
+        return;
+    }
+
+    // Update voltage scaling
+    g2.fwd_batt_cmp.update();
+
 #if AP_SCRIPTING_ENABLED
-    } else if (nav_scripting_active()) {
+    if (nav_scripting_active()) {
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
+    } else
 #endif
-    } else if (control_mode == &mode_stabilize ||
+           if (control_mode == &mode_stabilize ||
                control_mode == &mode_training ||
                control_mode == &mode_acro ||
                control_mode == &mode_fbwa ||
@@ -581,8 +599,7 @@ void Plane::set_throttle(void)
             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,
-                                            constrain_int16(get_adjusted_throttle_input(true), min_throttle, max_throttle));
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_adjusted_throttle_input(true));
         }
     } else if (control_mode->is_guided_mode() &&
                guided_throttle_passthru) {
@@ -591,21 +608,24 @@ void Plane::set_throttle(void)
 #if HAL_QUADPLANE_ENABLED
     } else if (quadplane.in_vtol_mode()) {
         float fwd_thr = 0;
-        // if armed and not spooled down ask quadplane code for forward throttle
-        if (quadplane.motors->armed() &&
-            quadplane.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN) {
-
-            fwd_thr = constrain_float(quadplane.forward_throttle_pct(), min_throttle, max_throttle);
+        // if enabled ask quadplane code for forward throttle
+        if (quadplane.allow_forward_throttle_in_vtol_mode()) {
+            fwd_thr = quadplane.forward_throttle_pct();
         }
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
 #endif  // HAL_QUADPLANE_ENABLED
 
     } else {
-        // Apply min/max limits and voltage compensation to throttle output from flight mode
+        // Apply voltage compensation to throttle output from flight mode
         const float throttle = g2.fwd_batt_cmp.apply_throttle(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle));
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_float(throttle, min_throttle, max_throttle));
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
     }
 
+    if (control_mode->use_throttle_limits()) {
+        // Apply min/max throttle limits
+        const float limited_throttle = apply_throttle_limits(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle));
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, limited_throttle);
+    }
 }
 
 /*