Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Peter Barker 2022-03-19 12:57:13 +11:00 committed by Andrew Tridgell
parent 8167995107
commit 43b5f22c1a
2 changed files with 58 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
Tools/autotest/arduplane.py
View File

@ -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",

18
Tools/autotest/common.py
View File

@ -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)