From 35cb526241bc7e147c262b3ed3957cea6a287328 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 19 Jun 2024 13:07:44 +1000 Subject: [PATCH] autotest: add a test for gripper release on thrust loss --- Tools/autotest/arducopter.py | 38 +++++++++++++++++++++++++++++------- 1 file changed, 31 insertions(+), 7 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 36c705e414..9a88c19714 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -4938,6 +4938,15 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): 0.01 # size of target in radians, Y-axis ) + def set_servo_gripper_parameters(self): + self.set_parameters({ + "GRIP_ENABLE": 1, + "GRIP_TYPE": 1, + "SIM_GRPS_ENABLE": 1, + "SIM_GRPS_PIN": 8, + "SERVO8_FUNCTION": 28, + }) + def PayloadPlaceMission(self): """Test payload placing in auto.""" self.context_push() @@ -4945,13 +4954,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): ex = None try: self.set_analog_rangefinder_parameters() - self.set_parameters({ - "GRIP_ENABLE": 1, - "GRIP_TYPE": 1, - "SIM_GRPS_ENABLE": 1, - "SIM_GRPS_PIN": 8, - "SERVO8_FUNCTION": 28, - }) + self.set_servo_gripper_parameters() self.reboot_sitl() self.load_mission("copter_payload_place.txt") @@ -11521,6 +11524,26 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.reboot_sitl() # because we set home + def GripperReleaseOnThrustLoss(self): + '''tests that gripper is released on thrust loss if option set''' + + self.context_push() + self.set_servo_gripper_parameters() + self.reboot_sitl() + + self.takeoff(30, mode='LOITER') + self.set_parameters({ + "SIM_ENGINE_FAIL": 1, + "SIM_ENGINE_MUL": 0.5, + "FLIGHT_OPTIONS": 4, + }) + + self.wait_statustext("Gripper Load Released", timeout=60) + + self.context_pop() + self.do_RTL() + self.reboot_sitl() + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -11611,6 +11634,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.GuidedForceArm, self.GuidedWeatherVane, self.Clamp, + self.GripperReleaseOnThrustLoss, ]) return ret