autotest: augment ADSB test for RC_Channel enable/disable

Tools/autotest/arduplane.py

@@ -1375,13 +1375,23 @@ class AutoTestPlane(AutoTest):
         self.wait_mode("CIRCLE")
         self.set_rc(9, 1000)
 
-    def test_adsb(self):
-        self.wait_ready_to_arm()
-        here = self.mav.location()
-        # message ADSB_VEHICLE 37 -353632614 1491652305 0 584070 0 0 0 "bob" 3 1 255 17
-        self.set_parameter("ADSB_ENABLE", 1)
-        self.set_parameter("AVD_ENABLE", 1)
-        self.delay_sim_time(2) # TODO: work out why this is required...
+    def wait_for_collision_threat_to_clear(self):
+        '''wait to get a "clear" collision message", then slurp remaining
+        messages'''
+        last_collision = self.get_sim_time()
+        while True:
+            now = self.get_sim_time()
+            if now - last_collision > 5:
+                return
+            self.progress("Waiting for collision message")
+            m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=1)
+            self.progress("Got (%s)" % str(m))
+            if m is None:
+                continue
+            last_collision = now
+
+    def test_adsb_send_threatening_adsb_message(self, here):
+        self.progress("Sending ABSD_VEHICLE message")
         self.mav.mav.adsb_vehicle_send(37, # ICAO address
                                        int(here.lat * 1e7),
                                        int(here.lng * 1e7),
@@ -1396,12 +1406,67 @@ class AutoTestPlane(AutoTest):
                                        65535, # flags
                                        17 # squawk
         )
-        self.progress("Waiting for collision message")
-        m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4)
-        if m is None:
-            raise NotAchievedException("Did not get collision message")
-        if m.threat_level != 2:
-            raise NotAchievedException("Expected some threat at least")
+
+    def test_adsb(self):
+        self.context_push()
+        ex = None
+        try:
+            # message ADSB_VEHICLE 37 -353632614 1491652305 0 584070 0 0 0 "bob" 3 1 255 17
+            self.set_parameter("RC12_OPTION", 38) # avoid-adsb
+            self.set_rc(12, 2000)
+            self.set_parameter("ADSB_ENABLE", 1)
+            self.set_parameter("AVD_ENABLE", 1)
+            self.set_parameter("AVD_F_ACTION", mavutil.mavlink.MAV_COLLISION_ACTION_RTL)
+            self.reboot_sitl()
+            self.wait_ready_to_arm()
+            here = self.mav.location()
+            self.change_mode("FBWA")
+            self.delay_sim_time(2) # TODO: work out why this is required...
+            self.test_adsb_send_threatening_adsb_message(here)
+            self.progress("Waiting for collision message")
+            m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4)
+            if m is None:
+                raise NotAchievedException("Did not get collision message")
+            if m.threat_level != 2:
+                raise NotAchievedException("Expected some threat at least")
+            if m.action != mavutil.mavlink.MAV_COLLISION_ACTION_RTL:
+                raise NotAchievedException("Incorrect action; want=%u got=%u" %
+                                           (mavutil.mavlink.MAV_COLLISION_ACTION_RTL, m.action))
+            self.wait_mode("RTL")
+
+            self.progress("Sending far-away ABSD_VEHICLE message")
+            self.mav.mav.adsb_vehicle_send(37, # ICAO address
+                                           int(here.lat+1 * 1e7),
+                                           int(here.lng * 1e7),
+                                           mavutil.mavlink.ADSB_ALTITUDE_TYPE_PRESSURE_QNH,
+                                           int(here.alt*1000 + 10000), # 10m up
+                                           0, # heading in cdeg
+                                           0, # horizontal velocity cm/s
+                                           0, # vertical velocity cm/s
+                                           "bob".encode("ascii"), # callsign
+                                           mavutil.mavlink.ADSB_EMITTER_TYPE_LIGHT,
+                                           1, # time since last communication
+                                           65535, # flags
+                                           17 # squawk
+            )
+            self.wait_for_collision_threat_to_clear()
+            self.change_mode("FBWA")
+
+            self.progress("Disabling ADSB-avoidance with RC channel")
+            self.set_rc(12, 1000)
+            self.delay_sim_time(1) # let the switch get polled
+            self.test_adsb_send_threatening_adsb_message(here)
+            m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4)
+            print("Got (%s)" % str(m))
+            if m is not None:
+                raise NotAchievedException("Got collision message when I shouldn't have")
+
+        except Exception as e:
+            ex = e
+        self.context_pop()
+        self.reboot_sitl()
+        if ex is not None:
+            raise ex
 
     def fly_do_guided_request(self, target_system=1, target_component=1):
         self.progress("Takeoff")