diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 3721eb8de32d7..597399cc21676 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -400,7 +400,7 @@ class ModeManual : public Mode void run() override; // true if throttle min/max limits should be applied - bool use_throttle_limits() const override { return false; } + bool use_throttle_limits() const override; // true if voltage correction should be applied to throttle bool use_battery_compensation() const override { return false; } diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 1f8fcd702cd14..8c5a8341023b1 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -6,22 +6,8 @@ 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)); output_rudder_and_steering(plane.rudder_in_expo(false)); - 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 + const float throttle = plane.get_throttle_input(true); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); plane.nav_roll_cd = ahrs.roll_sensor; @@ -32,3 +18,14 @@ void ModeManual::run() { reset_controllers(); } + +// true if throttle min/max limits should be applied +bool ModeManual::use_throttle_limits() const +{ +#if HAL_QUADPLANE_ENABLED + if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) { + return true; + } +#endif + return false; +} diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 531a63a24f9d4..3365d811472d1 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -5312,6 +5312,38 @@ def SagetechMXS(self): home = self.home_position_as_mav_location() self.assert_distance(home, adsb_vehicle_loc, 0, 10000) + def MinThrottle(self): + '''Make sure min throttle does not apply in manual mode and does in FBWA''' + + servo_min = self.get_parameter("RC3_MIN") + servo_max = self.get_parameter("RC3_MAX") + min_throttle = 10 + servo_min_throttle = servo_min + (servo_max - servo_min) * (min_throttle / 100) + + # Set min throttle + self.set_parameter('THR_MIN', min_throttle) + + # Should be 0 throttle while disarmed + self.change_mode("MANUAL") + self.drain_mav() # make sure we have the latest data before checking throttle output + self.assert_servo_channel_value(3, servo_min) + + # Arm and check throttle is still 0 in manual + self.wait_ready_to_arm() + self.arm_vehicle() + self.drain_mav() + self.assert_servo_channel_value(3, servo_min) + + # FBWA should apply throttle min + self.change_mode("FBWA") + self.drain_mav() + self.assert_servo_channel_value(3, servo_min_throttle) + + # But not when disarmed + self.disarm_vehicle() + self.drain_mav() + self.assert_servo_channel_value(3, servo_min) + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -5419,6 +5451,7 @@ def tests(self): self.TerrainRally, self.MAV_CMD_NAV_LOITER_UNLIM, self.MAV_CMD_NAV_RETURN_TO_LAUNCH, + self.MinThrottle, ]) return ret