From a4ec232337e0c8a86e0f227769eeaa088c4d9757 Mon Sep 17 00:00:00 2001
From: Clyde McQueen <clyde@mcqueen.net>
Date: Thu, 7 Mar 2024 09:38:41 -0800
Subject: [PATCH] Sub: test for ABOVE_TERRAIN frame

---
 .../TerrainMission/terrain_mission.txt        | 16 +++++++++++
 Tools/autotest/ardusub.py                     | 27 ++++++++++++++++++-
 2 files changed, 42 insertions(+), 1 deletion(-)
 create mode 100644 Tools/autotest/ArduSub_Tests/TerrainMission/terrain_mission.txt

diff --git a/Tools/autotest/ArduSub_Tests/TerrainMission/terrain_mission.txt b/Tools/autotest/ArduSub_Tests/TerrainMission/terrain_mission.txt
new file mode 100644
index 0000000000..98c44b11c0
--- /dev/null
+++ b/Tools/autotest/ArduSub_Tests/TerrainMission/terrain_mission.txt
@@ -0,0 +1,16 @@
+QGC WPL 110
+
+# Item 0 is the home position, frame=3
+0	0	3	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.393982	0.0	1
+
+# Descend to -25m, frame=3
+1	0	3	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.393982	-25.0	1
+
+# Travel at current rangefinder reading (~25m), frame=10
+2	0	10	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.394432	0.0	1
+
+# Descend to 10m above floor, frame=10
+3	0	10	16	0.000000	0.000000	0.000000	0.000000	33.810310	-118.394432	10.0	1
+
+# Ascend to 15m above floor, frame=10
+4	0	10	16	0.000000	0.000000	0.000000	0.000000	33.810310	-118.393982	15.0	1
diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index 73ef3e08c9..44beefaed3 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -1,6 +1,9 @@
 '''
 Dive ArduSub in SITL
 
+Depth of water is 50m, the ground is flat
+Parameters are in-code defaults plus default_params/sub.parm
+
 AP_FLAKE8_CLEAN
 '''
 
@@ -563,7 +566,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
     def MAV_CMD_DO_CHANGE_SPEED(self):
         '''ensure vehicle changes speeds when DO_CHANGE_SPEED received'''
         items = [
-            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, -3),  # Dive so we have constrat drag
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, -3),  # Dive so we have constant drag
             (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, -1),
             (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
         ]
@@ -621,6 +624,27 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
         self._MAV_CMD_CONDITION_YAW(self.run_cmd)
         self._MAV_CMD_CONDITION_YAW(self.run_cmd_int)
 
+    def TerrainMission(self):
+        """Mission using surface tracking"""
+
+        if self.get_parameter('RNGFND1_MAX_CM') != 3000.0:
+            raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0)
+
+        filename = "terrain_mission.txt"
+        self.progress("Executing mission %s" % filename)
+        self.load_mission(filename)
+        self.set_rc_default()
+        self.arm_vehicle()
+        self.change_mode('AUTO')
+        self.wait_waypoint(1, 4, max_dist=5)
+        self.delay_sim_time(3)
+
+        # Expect sub to hover at final altitude
+        self.assert_altitude(-36.0)
+
+        self.disarm_vehicle()
+        self.progress("Mission OK")
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestSub, self).tests()
@@ -644,6 +668,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
             self.MAV_CMD_MISSION_START,
             self.MAV_CMD_DO_CHANGE_SPEED,
             self.MAV_CMD_CONDITION_YAW,
+            self.TerrainMission,
         ])
 
         return ret