From 1340132f6fc242e5d4c678935a0f4091e6db7c2a Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 22 Oct 2021 08:55:42 +1100
Subject: [PATCH] autotest: add rangefinder test

---
 Tools/autotest/common.py |  9 ++++++---
 Tools/autotest/rover.py  | 25 +++++++++++++++++++++++++
 2 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 5f0ce97445..2974b276b5 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -4321,14 +4321,17 @@ class AutoTest(ABC):
             0,
             0)
 
-    def set_analog_rangefinder_parameters(self):
-        self.set_parameters({
+    def analog_rangefinder_parameters(self):
+        return {
             "RNGFND1_TYPE": 1,
             "RNGFND1_MIN_CM": 0,
             "RNGFND1_MAX_CM": 4000,
             "RNGFND1_SCALING": 12.12,
             "RNGFND1_PIN": 0,
-        })
+        }
+
+    def set_analog_rangefinder_parameters(self):
+        self.set_parameters(self.analog_rangefinder_parameters())
 
     def send_debug_trap(self, timeout=6000):
         self.progress("Sending trap to autopilot")
diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py
index 6eca1fc080..685739b05d 100644
--- a/Tools/autotest/rover.py
+++ b/Tools/autotest/rover.py
@@ -5785,6 +5785,27 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         # make sure we're back at our original value:
         self.assert_parameter_value("LOG_BITMASK", 1)
 
+    def RangeFinder(self):
+        # the following magic numbers correspond to the post locations in SITL
+        home_string = "%s,%s,%s,%s" % (51.8752066, 14.6487840, 54.15, 315)
+
+        self.set_parameters({
+            "SIM_SONAR_ROT": 6,
+            **(self.analog_rangefinder_parameters()),
+        })
+        self.customise_SITL_commandline([
+            "--home", home_string,
+        ])
+        self.wait_ready_to_arm()
+        if self.mavproxy is not None:
+            self.mavproxy.send('script /tmp/post-locations.scr\n')
+        m = self.assert_receive_message('RANGEFINDER', very_verbose=True)
+        if m.voltage == 0:
+            raise NotAchievedException("Did not get non-zero voltage")
+        want_range = 10
+        if abs(m.distance - want_range) > 0.1:
+            raise NotAchievedException("Expected %fm got %fm" % (want_range, m.distance))
+
     def test_depthfinder(self):
         # Setup rangefinders
         self.customise_SITL_commandline([
@@ -6083,6 +6104,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
              "Accelerometer Calibration testing",
              self.accelcal),
 
+            ("RangeFinder",
+             "Test RangeFinder",
+             self.RangeFinder),
+
             ("AP_Proximity_MAV",
              "Test MAV proximity backend",
              self.ap_proximity_mav),