From f141d3955b3af5df9315df2ed028130e20a89a56 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 27 Feb 2024 00:19:06 +0000 Subject: [PATCH] Tools: autotest: Plane: add min throttle test --- Tools/autotest/arduplane.py | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) 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