mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
autotest: add test for MAV_CMD_DO_LAND_START
This commit is contained in:
parent
ef1952e1c0
commit
1641aa42ec
@ -10308,6 +10308,44 @@ class AutoTestCopter(AutoTest):
|
|||||||
command(mavutil.mavlink.MAV_CMD_NAV_LAND)
|
command(mavutil.mavlink.MAV_CMD_NAV_LAND)
|
||||||
self.wait_mode('LAND')
|
self.wait_mode('LAND')
|
||||||
|
|
||||||
|
def start_flying_simple_rehome_mission(self, items):
|
||||||
|
'''uploads items, changes mode to auto, waits ready to arm and arms
|
||||||
|
vehicle. If the first item it a takeoff you can expect the
|
||||||
|
vehicle to fly after this method returns
|
||||||
|
'''
|
||||||
|
|
||||||
|
self.upload_simple_relhome_mission(items)
|
||||||
|
|
||||||
|
self.set_parameter("AUTO_OPTIONS", 3)
|
||||||
|
self.change_mode('AUTO')
|
||||||
|
self.wait_ready_to_arm()
|
||||||
|
|
||||||
|
self.arm_vehicle()
|
||||||
|
|
||||||
|
def _MAV_CMD_DO_LAND_START(self, run_cmd):
|
||||||
|
alt = 5
|
||||||
|
self.start_flying_simple_rehome_mission([
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, alt),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0),
|
||||||
|
(mavutil.mavlink.MAV_CMD_DO_LAND_START, 0, 0, alt),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 2000, alt),
|
||||||
|
(mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0),
|
||||||
|
])
|
||||||
|
|
||||||
|
self.wait_current_waypoint(2)
|
||||||
|
run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START)
|
||||||
|
self.wait_current_waypoint(5)
|
||||||
|
# we pretend to be in RTL mode while doing this:
|
||||||
|
self.wait_mode("AUTO_RTL")
|
||||||
|
self.do_RTL()
|
||||||
|
|
||||||
|
def MAV_CMD_DO_LAND_START(self):
|
||||||
|
'''test handling of mavlink-received MAV_CMD_DO_LAND_START command'''
|
||||||
|
self._MAV_CMD_DO_LAND_START(self.run_cmd)
|
||||||
|
self.zero_throttle()
|
||||||
|
self._MAV_CMD_DO_LAND_START(self.run_cmd_int)
|
||||||
|
|
||||||
def tests2b(self): # this block currently around 9.5mins here
|
def tests2b(self): # this block currently around 9.5mins here
|
||||||
'''return list of all tests'''
|
'''return list of all tests'''
|
||||||
ret = ([
|
ret = ([
|
||||||
@ -10373,6 +10411,7 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.SafetySwitch,
|
self.SafetySwitch,
|
||||||
self.BrakeZ,
|
self.BrakeZ,
|
||||||
self.MAV_CMD_DO_FLIGHTTERMINATION,
|
self.MAV_CMD_DO_FLIGHTTERMINATION,
|
||||||
|
self.MAV_CMD_DO_LAND_START,
|
||||||
])
|
])
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user