autotest: add test for avoidance resume-auto-or-loiter action

Tools/autotest/arduplane.py

@@ -1970,7 +1970,49 @@ function'''
         self.reboot_sitl()
         self.assert_receive_message('ADSB_VEHICLE', timeout=30)
 
-    def test_adsb(self):
+    def ADSB(self):
+        self.ADSB_f_action_rtl()
+        self.ADSB_r_action_resume_or_loiter()
+
+    def ADSB_r_action_resume_or_loiter(self):
+        '''ensure we resume auto mission or enter loiter'''
+        self.set_parameters({
+            "ADSB_TYPE": 1,
+            "AVD_ENABLE": 1,
+            "AVD_F_ACTION": mavutil.mavlink.MAV_COLLISION_ACTION_MOVE_HORIZONTALLY,
+            "AVD_F_RCVRY": 3,  # resume auto or loiter
+        })
+        self.reboot_sitl()
+        self.takeoff(50)
+        # fly North, create thread to east, wait for flying east
+        self.start_subtest("Testing loiter resume")
+        self.reach_heading_manual(0)
+        here = self.mav.location()
+        self.test_adsb_send_threatening_adsb_message(here, offset_ne=(0, 30))
+        self.wait_mode('AVOID_ADSB')
+        # recovery has the vehicle circling a point... but we don't
+        # know which point.  So wait 'til it looks like it is
+        # circling, then grab the point, then check we're circling
+        # it...
+        self.wait_heading(290)
+        self.wait_heading(300)
+        dest = self.position_target_loc()
+        REALLY_BAD_FUDGE_FACTOR = 1.25  # FIXME
+        expected_radius = REALLY_BAD_FUDGE_FACTOR * self.get_parameter('WP_LOITER_RAD')
+        self.wait_circling_point_with_radius(dest, expected_radius)
+
+        self.start_subtest("Testing mission resume")
+        self.reach_heading_manual(270)
+        self.load_generic_mission("CMAC-circuit.txt", strict=False)
+        self.change_mode('AUTO')
+        self.wait_current_waypoint(2)
+        self.test_adsb_send_threatening_adsb_message(here, offset_ne=(0, 30))
+        self.wait_mode('AVOID_ADSB')
+        self.wait_mode('AUTO')
+
+        self.fly_home_land_and_disarm()
+
+    def ADSB_f_action_rtl(self):
         self.context_push()
         ex = None
         try:
@@ -3616,7 +3658,7 @@ function'''
 
             ("ADSB",
              "Test ADSB",
-             self.test_adsb),
+             self.ADSB),
 
             ("SimADSB",
              "Test SIM_ADSB",

Tools/autotest/common.py

@@ -2009,12 +2009,15 @@ class AutoTest(ABC):
                 htree[n] = p
         return htree
 
-    def test_adsb_send_threatening_adsb_message(self, here):
+    def test_adsb_send_threatening_adsb_message(self, here, offset_ne=None):
         self.progress("Sending ABSD_VEHICLE message")
+        new = here
+        if offset_ne is not None:
+            new = self.offset_location_ne(new, offset_ne[0], offset_ne[1])
         self.mav.mav.adsb_vehicle_send(
             37, # ICAO address
-            int(here.lat * 1e7),
-            int(here.lng * 1e7),
+            int(new.lat * 1e7),
+            int(new.lng * 1e7),
             mavutil.mavlink.ADSB_ALTITUDE_TYPE_PRESSURE_QNH,
             int(here.alt*1000 + 10000), # 10m up
             0, # heading in cdeg
@@ -5367,7 +5370,9 @@ class AutoTest(ABC):
             self.wait_heading(heading)
             self.set_rc(4, 1500)
         if self.is_plane():
-            self.progress("NOT IMPLEMENTED")
+            self.set_rc(1, 1800)
+            self.wait_heading(heading)
+            self.set_rc(1, 1500)
         if self.is_rover():
             steering_pwm = 1700
             if not turn_right:
@@ -7219,6 +7224,11 @@ Also, ignores heartbeats not from our target system'''
         self.progress("Polled home position (%s)" % str(m))
         return m
 
+    def position_target_loc(self):
+        '''returns target location based on POSITION_TARGET_GLOBAL_INT'''
+        m = self.mav.messages.get("POSITION_TARGET_GLOBAL_INT", None)
+        return mavutil.location(m.lat_int*1e-7, m.lon_int*1e-7, m.alt)
+
     def distance_to_nav_target(self, use_cached_nav_controller_output=False):
         '''returns distance to waypoint navigation target in metres'''
         m = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)