diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index dcf351fb70..82b2618038 100644 --- a/Tools/autotest/arduplane.py +++ b/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", diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 2974b276b5..b33851a965 100644 --- a/Tools/autotest/common.py +++ b/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)