From 3f53395d3d10ab7de7d303e35213c9d5814e35aa Mon Sep 17 00:00:00 2001
From: Clyde McQueen <clyde@mcqueen.net>
Date: Tue, 18 Jul 2023 13:14:47 -0700
Subject: [PATCH] Test RNG_HOLD

---
 Tools/autotest/ardusub.py | 34 ++++++++++++++++++++++++++++++++--
 1 file changed, 32 insertions(+), 2 deletions(-)

diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index a6af0aba9c06c6..b815dd47edddc7 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -114,7 +114,7 @@ def AltitudeHold(self):
             raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
         pwm = 1300
         if msg.relative_alt/1000.0 < -6.0:
-            # need to g`o up, not down!
+            # need to go up, not down!
             pwm = 1700
         self.set_rc(Joystick.Throttle, pwm)
         self.wait_altitude(altitude_min=-6, altitude_max=-5)
@@ -166,8 +166,33 @@ def AltitudeHold(self):
         self.watch_altitude_maintained()
         self.disarm_vehicle()
 
+    def RangeHold(self):
+        """Test RNG_HOLD mode"""
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.change_mode('ALT_HOLD')
+
+        # Dive so that rangefinder can see the seafloor
+        msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
+        if msg is None:
+            raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
+        pwm = 1300
+        if msg.relative_alt/1000.0 < -41.0:
+            pwm = 1700
+        self.set_rc(Joystick.Throttle, pwm)
+        self.wait_altitude(altitude_min=-41, altitude_max=-40, relative=False, timeout=60)
+        self.set_rc(Joystick.Throttle, 1500)
+
+        # Let the vehicle settle
+        self.delay_sim_time(1)
+
+        # Switch to RNG_HOLD
+        self.change_mode(21)
+        self.watch_altitude_maintained()
+        self.disarm_vehicle()
+
     def ModeChanges(self, delta=0.2):
-        """Check if alternating between ALTHOLD, STABILIZE and POSHOLD affects altitude"""
+        """Check if alternating between ALTHOLD, STABILIZE, POSHOLD and RNG_HOLD (mode 21) affects altitude"""
         self.wait_ready_to_arm()
         self.arm_vehicle()
         # zero buoyancy and no baro noise
@@ -187,12 +212,16 @@ def ModeChanges(self, delta=0.2):
         self.delay_sim_time(2)
         self.change_mode('STABILIZE')
         self.delay_sim_time(2)
+        self.change_mode(21)
+        self.delay_sim_time(2)
         self.change_mode('ALT_HOLD')
         self.delay_sim_time(2)
         self.change_mode('STABILIZE')
         self.delay_sim_time(2)
         self.change_mode('ALT_HOLD')
         self.delay_sim_time(2)
+        self.change_mode(21)
+        self.delay_sim_time(2)
         self.change_mode('MANUAL')
         self.disarm_vehicle()
         final_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt
@@ -432,6 +461,7 @@ def tests(self):
         ret.extend([
             self.DiveManual,
             self.AltitudeHold,
+            self.RangeHold,
             self.PositionHold,
             self.ModeChanges,
             self.DiveMission,