From a06a192255d7f287439787d5ec03b35c560b0967 Mon Sep 17 00:00:00 2001
From: Josh Henderson <hendjoshsr71@gmail.com>
Date: Thu, 14 Oct 2021 00:31:24 -0400
Subject: [PATCH] Autotest: Rover add test for multiple depth finders

---
 .../ArduRover_Tests/DepthFinder/rover1.txt    |  6 ++
 Tools/autotest/rover.py                       | 77 +++++++++++++++++++
 2 files changed, 83 insertions(+)
 create mode 100644 Tools/autotest/ArduRover_Tests/DepthFinder/rover1.txt

diff --git a/Tools/autotest/ArduRover_Tests/DepthFinder/rover1.txt b/Tools/autotest/ArduRover_Tests/DepthFinder/rover1.txt
new file mode 100644
index 0000000000..ae46ae67be
--- /dev/null
+++ b/Tools/autotest/ArduRover_Tests/DepthFinder/rover1.txt
@@ -0,0 +1,6 @@
+QGC WPL 110
+0	1	0	16	0	0	0	0	40.071377	-105.229790	1583.700000	1
+1	0	3	16	0.00000000	0.00000000	0.00000000	0.00000000	40.07203730	-105.22771600	50.000000	1
+2	0	3	16	0.00000000	0.00000000	0.00000000	0.00000000	40.07168840	-105.22739950	50.000000	1
+3	0	3	16	0.00000000	0.00000000	0.00000000	0.00000000	40.07163090	-105.22847240	50.000000	1
+4	0	3	16	0.00000000	0.00000000	0.00000000	0.00000000	40.07118340	-105.22971150	50.000000	1
diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py
index 4952262690..0600637cf9 100644
--- a/Tools/autotest/rover.py
+++ b/Tools/autotest/rover.py
@@ -5720,6 +5720,79 @@ 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 test_depthfinder(self):
+        # Setup rangefinders
+        self.customise_SITL_commandline([
+            "--uartH=sim:nmea", # NMEA Rangefinder
+        ])
+
+        # RANGEFINDER_INSTANCES = [0, 2, 5]
+        self.set_parameters({
+            "RNGFND1_TYPE" : 17,     # NMEA must attach uart to SITL
+            "RNGFND1_ORIENT" : 25,   # Set to downward facing
+            "SERIAL7_PROTOCOL" : 9,  # Rangefinder on uartH
+
+            "RNGFND3_TYPE" : 2,      # MaxbotixI2C
+            "RNGFND3_ADDR" : 112,    # 0x70 address from SIM_I2C.cpp
+            "RNGFND3_ORIENT" : 0,    # Set to forward facing, thus we should not receive DPTH messages from this one
+
+            "RNGFND6_ADDR" : 113,    # 0x71 address from SIM_I2C.cpp
+            "RNGFND6_ORIENT" : 25,   # Set to downward facing
+            "RNGFND6_TYPE" : 2,      # MaxbotixI2C
+        })
+
+        self.reboot_sitl()
+        self.wait_ready_to_arm()
+
+        # should not get WATER_DEPTH messages or DPTH logs when the FRAME_CLASS is not a boat
+        m = self.mav.recv_match(type="WATER_DEPTH", blocking=True, timeout=2)
+        if m is not None:
+            raise NotAchievedException("WATER_DEPTH: received message when FRAME_CLASS not a Boat")
+
+        # Set FRAME_CLASS to start receiving WATER_DEPTH messages & logging DPTH
+        self.set_parameters({
+            "FRAME_CLASS": 2,       # Boat
+        })
+
+        # Check each rangefinder instance is in collection
+        rangefinder = [None, None, None, None, None, None] # Be lazy FIXME only need [3]
+
+        def check_rangefinder(mav, m):
+            if m.get_type() != 'WATER_DEPTH':
+                return
+
+            id = m.id
+
+            # Should not find instance 3 as it is forward facing
+            if id == 2:
+                raise NotAchievedException("Depthfinder Instance %i with non-downward orientation found" % (id))
+
+            rangefinder[id] = True
+
+            if id == 0:
+                if float(m.temperature) == 0.0:
+                    raise NotAchievedException("Depthfinder Instance %i NMEA with temperature not found" % (id))
+            elif id == 5:
+                if float(m.temperature) != 0.0:
+                    raise NotAchievedException("Depthfinder Instance %i should not have temperature" % (id))
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        self.install_message_hook_context(check_rangefinder)
+        self.drive_mission("rover1.txt", strict=False)
+
+        if rangefinder[0] is None:
+            raise NotAchievedException("Never saw Depthfinder 1")
+        if rangefinder[2] is not None:
+            raise NotAchievedException("Should not have found a Depthfinder 3")
+        if rangefinder[5] is None:
+            raise NotAchievedException("Never saw Depthfinder 6")
+        if not self.current_onboard_log_contains_message("DPTH"):
+            raise NotAchievedException("Expected DPTH log message")
+
+        # self.context_pop()
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestRover, self).tests()
@@ -5953,6 +6026,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             ("LogUpload",
              "Upload logs",
              self.log_upload),
+
+            ("DepthFinder",
+             "Test mulitple depthfinders for boats",
+             self.test_depthfinder),
         ])
         return ret