From 02655fec78761dc6196320603cfb9e67540a2dcc Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 20 Jun 2024 18:54:25 +1000
Subject: [PATCH] autotest: add test for automated docking clamp release

---
 Tools/autotest/arducopter.py | 67 ++++++++++++++++++++++++++++++++++++
 1 file changed, 67 insertions(+)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index a52c799c5ba1a4..f19bbdca6d5716 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -11514,6 +11514,72 @@ def Clamp(self):
 
         self.reboot_sitl()  # because we set home
 
+    def TakeoffClampRelease(self):
+        '''check automated docking clamp release'''
+        self.context_push()
+        clamp_ch = 11
+        rc_clamp_ch = 9
+        self.set_parameters({
+            "SIM_CLAMP_CH": clamp_ch,
+            f"SERVO{clamp_ch}_FUNCTION": 156,
+            f"RC{rc_clamp_ch}_OPTION": 179,
+        })
+        self.reboot_sitl()
+        self.context_collect('STATUSTEXT')
+
+        self.start_subtest("grab with aux function")
+        self.run_auxfunc(179, 2)
+        self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)
+        self.run_auxfunc(179, 0)
+        self.wait_statustext("SITL: Clamp: released vehicle", check_context=True)
+
+        self.start_subtest("grab with RC")
+        self.set_rc(rc_clamp_ch, 2000)
+        self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)
+        self.set_rc(rc_clamp_ch, 1000)
+        self.wait_statustext("SITL: Clamp: released vehicle", check_context=True)
+
+        self.start_subtest("should not be able to take off without the option bit set")
+        self.run_auxfunc(179, 2)
+        self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.change_mode('STABILIZE')
+        self.set_rc(3, 2000)
+        self.wait_altitude(0, 5, minimum_duration=5, relative=True)
+        self.zero_throttle()
+        self.delay_sim_time(1)
+        self.disarm_vehicle()
+
+        self.progress("Setting flight options bit")
+        current_value = int(self.get_parameter("FLIGHT_OPTIONS"))
+        current_value |= 8  # enable clamp release
+        self.set_parameter("FLIGHT_OPTIONS", current_value)
+
+        self.start_subtest("should be able to take off with the option bit set")
+        self.takeoff(10, mode='GUIDED')
+        self.do_RTL()
+        self.zero_throttle()
+
+        self.start_subtest("should be able to take off in auto mode with option bit set")
+        self.run_auxfunc(179, 0)
+        self.run_auxfunc(179, 2)
+        self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)
+        self.upload_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
+            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
+        ])
+        self.set_parameters({
+            "AUTO_OPTIONS": 3,
+        })
+        self.change_mode('AUTO')
+        self.arm_vehicle()
+        self.wait_altitude(0, 5, minimum_duration=5, relative=True)
+        self.wait_disarmed()
+
+        self.context_pop()
+        self.reboot_sitl()
+
     def GripperReleaseOnThrustLoss(self):
         '''tests that gripper is released on thrust loss if option set'''
 
@@ -11625,6 +11691,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.GuidedWeatherVane,
             self.Clamp,
             self.GripperReleaseOnThrustLoss,
+            self.TakeoffClampRelease,
         ])
         return ret