mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
autotest: added quadplane ShipLanding test
This commit is contained in:
parent
c210675e95
commit
6a67830556
@ -0,0 +1,3 @@
|
|||||||
|
QGC WPL 110
|
||||||
|
0 0 0 16 0.000000 0.000000 0.000000 0.000000 37.625620 -122.332354 0.100000 1
|
||||||
|
1 0 3 84 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 1
|
@ -1341,11 +1341,11 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
|
|||||||
|
|
||||||
self.install_applet_script(applet_script)
|
self.install_applet_script(applet_script)
|
||||||
|
|
||||||
target = mavutil.location(-27.27440414, 151.28984285, 342.8, 0)
|
here = self.mav.location()
|
||||||
|
target = self.offset_location_ne(here, 20, 0)
|
||||||
|
|
||||||
self.set_parameters({
|
self.set_parameters({
|
||||||
"SCR_ENABLE": 1,
|
"SCR_ENABLE": 1,
|
||||||
"SIM_SPEEDUP": 4,
|
|
||||||
"PLND_ENABLED": 1,
|
"PLND_ENABLED": 1,
|
||||||
"PLND_TYPE": 4,
|
"PLND_TYPE": 4,
|
||||||
"SIM_PLD_ENABLE": 1,
|
"SIM_PLD_ENABLE": 1,
|
||||||
@ -1367,6 +1367,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
|
|||||||
self.context_collect('STATUSTEXT')
|
self.context_collect('STATUSTEXT')
|
||||||
self.set_parameters({
|
self.set_parameters({
|
||||||
"PLND_ALT_CUTOFF" : 5,
|
"PLND_ALT_CUTOFF" : 5,
|
||||||
|
"SIM_SPEEDUP" : 10,
|
||||||
})
|
})
|
||||||
|
|
||||||
self.scripting_restart()
|
self.scripting_restart()
|
||||||
@ -1393,6 +1394,61 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
|
|||||||
self.remove_installed_script(applet_script)
|
self.remove_installed_script(applet_script)
|
||||||
self.reboot_sitl()
|
self.reboot_sitl()
|
||||||
|
|
||||||
|
def ShipLanding(self):
|
||||||
|
'''ship landing test'''
|
||||||
|
applet_script = "plane_ship_landing.lua"
|
||||||
|
|
||||||
|
self.install_applet_script(applet_script)
|
||||||
|
|
||||||
|
self.set_parameters({
|
||||||
|
"SCR_ENABLE": 1,
|
||||||
|
"SIM_SHIP_ENABLE": 1,
|
||||||
|
"SIM_SHIP_SPEED": 5,
|
||||||
|
"SIM_SHIP_DSIZE": 10,
|
||||||
|
"FOLL_ENABLE": 1,
|
||||||
|
"FOLL_SYSID": 17,
|
||||||
|
"FOLL_OFS_TYPE": 1,
|
||||||
|
"SIM_TERRAIN" : 0,
|
||||||
|
"TERRAIN_ENABLE" : 0,
|
||||||
|
})
|
||||||
|
|
||||||
|
self.load_mission("takeoff100.txt")
|
||||||
|
|
||||||
|
self.reboot_sitl(check_position=False)
|
||||||
|
|
||||||
|
self.context_push()
|
||||||
|
self.context_collect('STATUSTEXT')
|
||||||
|
self.set_parameters({
|
||||||
|
"SHIP_ENABLE" : 1,
|
||||||
|
"SIM_SPEEDUP" : 10,
|
||||||
|
})
|
||||||
|
|
||||||
|
self.scripting_restart()
|
||||||
|
self.wait_text("ShipLanding: loaded", check_context=True)
|
||||||
|
|
||||||
|
self.wait_ready_to_arm()
|
||||||
|
self.change_mode("AUTO")
|
||||||
|
self.arm_vehicle()
|
||||||
|
self.wait_altitude(95, 105, relative=True, timeout=90)
|
||||||
|
self.drain_mav()
|
||||||
|
|
||||||
|
self.wait_text("Mission complete, changing mode to RTL", check_context=True, timeout=60)
|
||||||
|
self.wait_text("Descending for approach", check_context=True, timeout=60)
|
||||||
|
self.wait_text("Reached target altitude", check_context=True, timeout=120)
|
||||||
|
self.wait_text("Starting approach", check_context=True, timeout=120)
|
||||||
|
self.wait_text("Land complete", check_context=True, timeout=120)
|
||||||
|
|
||||||
|
self.wait_disarmed(timeout=180)
|
||||||
|
|
||||||
|
# we confirm successful landing on the ship from our ground speed. The
|
||||||
|
# deck is just 10m in size, so we must be within 10m if we are moving
|
||||||
|
# with the deck
|
||||||
|
self.wait_groundspeed(4.8, 5.2)
|
||||||
|
|
||||||
|
self.context_pop()
|
||||||
|
self.remove_installed_script(applet_script)
|
||||||
|
self.reboot_sitl(check_position=False)
|
||||||
|
|
||||||
def RCDisableAirspeedUse(self):
|
def RCDisableAirspeedUse(self):
|
||||||
'''check disabling airspeed using RC switch'''
|
'''check disabling airspeed using RC switch'''
|
||||||
self.set_parameter("RC9_OPTION", 106)
|
self.set_parameter("RC9_OPTION", 106)
|
||||||
@ -1637,6 +1693,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
|
|||||||
self.VTOLLandSpiral,
|
self.VTOLLandSpiral,
|
||||||
self.VTOLQuicktune,
|
self.VTOLQuicktune,
|
||||||
self.PrecisionLanding,
|
self.PrecisionLanding,
|
||||||
|
self.ShipLanding,
|
||||||
Test(self.MotorTest, kwargs={ # tests motors 4 and 2
|
Test(self.MotorTest, kwargs={ # tests motors 4 and 2
|
||||||
"mot1_servo_chan": 8, # quad-x second motor cw from f-r
|
"mot1_servo_chan": 8, # quad-x second motor cw from f-r
|
||||||
"mot4_servo_chan": 6, # quad-x third motor cw from f-r
|
"mot4_servo_chan": 6, # quad-x third motor cw from f-r
|
||||||
|
@ -2002,14 +2002,14 @@ class TestSuite(ABC):
|
|||||||
0,
|
0,
|
||||||
0)
|
0)
|
||||||
|
|
||||||
def reboot_sitl(self, required_bootcount=None, force=False):
|
def reboot_sitl(self, required_bootcount=None, force=False, check_position=True):
|
||||||
"""Reboot SITL instance and wait for it to reconnect."""
|
"""Reboot SITL instance and wait for it to reconnect."""
|
||||||
if self.armed() and not force:
|
if self.armed() and not force:
|
||||||
raise NotAchievedException("Reboot attempted while armed")
|
raise NotAchievedException("Reboot attempted while armed")
|
||||||
self.progress("Rebooting SITL")
|
self.progress("Rebooting SITL")
|
||||||
self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force)
|
self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force)
|
||||||
self.do_heartbeats(force=True)
|
self.do_heartbeats(force=True)
|
||||||
if self.frame != 'sailboat': # sailboats drift with wind!
|
if check_position and self.frame != 'sailboat': # sailboats drift with wind!
|
||||||
self.assert_simstate_location_is_at_startup_location()
|
self.assert_simstate_location_is_at_startup_location()
|
||||||
|
|
||||||
def reboot_sitl_mavproxy(self, required_bootcount=None):
|
def reboot_sitl_mavproxy(self, required_bootcount=None):
|
||||||
|
Loading…
Reference in New Issue
Block a user