From 1e7a5e58834efd04f75f9a1f671fff15cdf66d3f Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 19 Oct 2023 20:51:44 +1100
Subject: [PATCH] autotest: add simple test for ship operations

---
 .../ShipOps/ShipLanding.param                 | 45 +++++++++++++++++++
 Tools/autotest/arducopter.py                  | 45 +++++++++++++++++++
 2 files changed, 90 insertions(+)
 create mode 100644 Tools/autotest/ArduCopter_Tests/ShipOps/ShipLanding.param

diff --git a/Tools/autotest/ArduCopter_Tests/ShipOps/ShipLanding.param b/Tools/autotest/ArduCopter_Tests/ShipOps/ShipLanding.param
new file mode 100644
index 0000000000000..5e22c64a3380f
--- /dev/null
+++ b/Tools/autotest/ArduCopter_Tests/ShipOps/ShipLanding.param
@@ -0,0 +1,45 @@
+FOLL_ENABLE,1
+FOLL_OFS_TYPE,1
+FOLL_OFS_X,0
+FOLL_OFS_Y,0
+FOLL_OFS_Z,0
+FOLL_POS_P,0.1
+FOLL_SYSID,17
+FOLL_YAW_BEHAVE,2
+BATT_FS_LOW_ACT,7
+FS_GCS_ENABLE,7
+FS_THR_ENABLE,7
+FS_EKF_ACTION,1
+GRIP_ENABLE,1
+GRIP_GRAB,1000
+GRIP_NEUTRAL,1000
+GRIP_REGRAB,0
+GRIP_RELEASE,2000
+GRIP_TYPE,1
+GRIP_CAN_ID,0
+INS_GYR_CAL,0
+LOG_BITMASK,176127
+PSC_ANGLE_MAX,25
+RC6_OPTION,174
+RC7_OPTION,175
+RC8_OPTION,19
+SERVO9_FUNCTION,146
+SERVO9_MAX,2000
+SERVO9_MIN,1000
+SERVO9_REVERSED,1
+SERVO9_TRIM,1500
+SERVO10_FUNCTION,28
+SERVO10_MAX,2000
+SERVO10_MIN,1000
+SERVO10_REVERSED,0
+SERVO10_TRIM,1500
+SIM_SHIP_DSIZE,10
+SIM_SHIP_ENABLE,1
+SIM_SHIP_OFS_X,0
+SIM_SHIP_OFS_Y,0
+SIM_SHIP_OFS_Z,0
+SIM_SHIP_PSIZE,1000
+SIM_SHIP_SPEED,3
+SIM_SHIP_SYSID,17
+WPNAV_SPEED_DN,250
+WPNAV_SPEED_UP,250
diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 20c106bf4b9fe..417ab47b74b82 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -8001,6 +8001,50 @@ def ShipTakeoff(self):
         # ship will have moved on, so we land on the water which isn't moving
         self.wait_groundspeed(0, 2)
 
+    def ShipOps(self):
+        '''Fly Simulated Ship Operations'''
+        self.load_params_file("ShipLanding.param")
+
+        self.progress("Change to ShipOps mode")
+        self.change_mode(29)  # 29 is ship ops
+        self.wait_ready_to_arm()
+
+        ship_speed = self.get_parameter('SIM_SHIP_SPEED')
+
+        self.progress("ensure we are be moving with the ship")
+        self.wait_groundspeed(ship_speed-0.1, ship_speed+0.1)
+
+        self.progress("arm vehicle and make sure we continue to move with ship")
+        self.arm_vehicle()
+        self.wait_groundspeed(ship_speed-0.1, ship_speed+0.1, minimum_duration=2)
+
+        self.progress("put system into launch/retrieve mode")
+        aux_func_ship_ops = 175
+        self.run_auxfunc(aux_func_ship_ops, 2)
+
+        abs_alt = self.get_altitude(relative=False)
+
+        perch_alt = self.get_parameter('SHIP_PCH_ALT')
+
+#        self.set_parameter("SIM_SPEEDUP", 1)
+
+        self.progress("trigger launch")
+        self.set_rc(3, 2000)
+
+        self.wait_altitude(perch_alt-0.5, perch_alt+0.5, minimum_duration=5, relative=True, timeout=30)
+
+        # should retain our groundspeed to match the vehicle we are following
+        self.wait_groundspeed(ship_speed-0.1, ship_speed+0.1, minimum_duration=10)
+
+        # check offsets here
+#        self.mavproxy.interact()
+        self.progress("trigger recovery")
+        self.set_rc(3, 1000)
+
+        self.wait_altitude(abs_alt-5, abs_alt+5, minimum_duration=5, timeout=60)
+        self.wait_disarmed()
+
+
     def ParameterValidation(self):
         '''Test parameters are checked for validity'''
         # wait 10 seconds for initialisation
@@ -9807,6 +9851,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.FlyMissionTwice,
             self.IMUConsistency,
             self.AHRSTrimLand,
+            self.ShipOps,
         ])
         return ret