mirror of https://github.com/ArduPilot/ardupilot
autotest: add a test for RTL_AUTOLAND=1 behaviour
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
b03b8309ef
commit
ae3c74b8d9
|
|
@ -1662,6 +1662,115 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
|
||||||
# Force disarm
|
# Force disarm
|
||||||
self.disarm_vehicle(force=True)
|
self.disarm_vehicle(force=True)
|
||||||
|
|
||||||
|
def RTL_AUTOLAND_1(self):
|
||||||
|
'''test behaviour when RTL_AUTOLAND==1'''
|
||||||
|
|
||||||
|
self.set_parameters({
|
||||||
|
"RTL_AUTOLAND": 1,
|
||||||
|
})
|
||||||
|
|
||||||
|
# when RTL is entered and RTL_AUTOLAND is 1 we should fly home
|
||||||
|
# then to the landing sequence. This mission puts the landing
|
||||||
|
# sequence well to the West of home so if we go directly there
|
||||||
|
# we won't come within 200m of home
|
||||||
|
wps = self.create_simple_relhome_mission([
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
|
||||||
|
# fly North
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30),
|
||||||
|
# add a waypoint 1km North (which we will look for and trigger RTL
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30),
|
||||||
|
|
||||||
|
# *exciting* landing sequence is ~1km West and points away from Home.
|
||||||
|
self.create_MISSION_ITEM_INT(
|
||||||
|
mavutil.mavlink.MAV_CMD_DO_LAND_START,
|
||||||
|
),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0),
|
||||||
|
])
|
||||||
|
self.check_mission_upload_download(wps)
|
||||||
|
|
||||||
|
self.change_mode('AUTO')
|
||||||
|
self.wait_ready_to_arm()
|
||||||
|
|
||||||
|
self.arm_vehicle()
|
||||||
|
self.wait_current_waypoint(3) # will be 2km North here
|
||||||
|
self.change_mode('RTL')
|
||||||
|
|
||||||
|
self.wait_distance_to_home(100, 200, timeout=120)
|
||||||
|
self.wait_current_waypoint(7)
|
||||||
|
|
||||||
|
self.fly_home_land_and_disarm()
|
||||||
|
|
||||||
|
def send_reposition_to_loc(self, loc):
|
||||||
|
self.run_cmd_int(
|
||||||
|
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
|
||||||
|
0,
|
||||||
|
1, # reposition flags; 1 means "change to guided"
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
int(loc.lat * 1e7),
|
||||||
|
int(loc.lng * 1e7),
|
||||||
|
20, # alt
|
||||||
|
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
|
||||||
|
)
|
||||||
|
|
||||||
|
def reposition_to_loc(self, loc, accuracy=100):
|
||||||
|
self.send_reposition_to_loc(loc)
|
||||||
|
self.wait_location(
|
||||||
|
loc,
|
||||||
|
accuracy=accuracy,
|
||||||
|
minimum_duration=20,
|
||||||
|
timeout=120,
|
||||||
|
)
|
||||||
|
|
||||||
|
def RTL_AUTOLAND_1_FROM_GUIDED(self):
|
||||||
|
'''test behaviour when RTL_AUTOLAND==1 and entering from guided'''
|
||||||
|
|
||||||
|
self.set_parameters({
|
||||||
|
"RTL_AUTOLAND": 1,
|
||||||
|
})
|
||||||
|
|
||||||
|
# when RTL is entered and RTL_AUTOLAND is 1 we should fly home
|
||||||
|
# then to the landing sequence. This mission puts the landing
|
||||||
|
# sequence well to the West of home so if we go directly there
|
||||||
|
# we won't come within 200m of home
|
||||||
|
wps = self.create_simple_relhome_mission([
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
|
||||||
|
# fly North
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30),
|
||||||
|
# add a waypoint 1km North (which we will look for and trigger RTL
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30),
|
||||||
|
|
||||||
|
# *exciting* landing sequence is ~1km West and points away from Home.
|
||||||
|
self.create_MISSION_ITEM_INT(
|
||||||
|
mavutil.mavlink.MAV_CMD_DO_LAND_START,
|
||||||
|
),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0),
|
||||||
|
])
|
||||||
|
self.check_mission_upload_download(wps)
|
||||||
|
self.set_current_waypoint(0, check_afterwards=False)
|
||||||
|
|
||||||
|
self.change_mode('AUTO')
|
||||||
|
self.wait_ready_to_arm()
|
||||||
|
|
||||||
|
here = self.mav.location()
|
||||||
|
guided_loc = self.offset_location_ne(here, 500, -500)
|
||||||
|
|
||||||
|
self.arm_vehicle()
|
||||||
|
self.wait_current_waypoint(3) # will be 2km North here
|
||||||
|
self.reposition_to_loc(guided_loc)
|
||||||
|
self.send_cmd_do_set_mode('RTL')
|
||||||
|
|
||||||
|
self.wait_distance_to_home(100, 200, timeout=120)
|
||||||
|
self.wait_current_waypoint(7)
|
||||||
|
|
||||||
|
self.fly_home_land_and_disarm()
|
||||||
|
|
||||||
def tests(self):
|
def tests(self):
|
||||||
'''return list of all tests'''
|
'''return list of all tests'''
|
||||||
|
|
||||||
|
|
@ -1706,5 +1815,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
|
||||||
self.MAV_CMD_NAV_TAKEOFF,
|
self.MAV_CMD_NAV_TAKEOFF,
|
||||||
self.Q_GUIDED_MODE,
|
self.Q_GUIDED_MODE,
|
||||||
self.DCMClimbRate,
|
self.DCMClimbRate,
|
||||||
|
self.RTL_AUTOLAND_1, # as in fly-home then go to landing sequence
|
||||||
|
self.RTL_AUTOLAND_1_FROM_GUIDED, # as in fly-home then go to landing sequence
|
||||||
])
|
])
|
||||||
return ret
|
return ret
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue