mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 23:33:57 -04:00
Tools: common: add a method to try arming and wait a result
This commit is contained in:
parent
59de90d97f
commit
b72a17533c
@ -2499,6 +2499,33 @@ class AutoTest(ABC):
|
||||
0,
|
||||
timeout=timeout)
|
||||
|
||||
def try_arm(self, result=True, expect_msg=None, timeout=60):
|
||||
"""Send Arming command, wait for the expected result and statustext."""
|
||||
self.progress("Try arming and wait for expected result")
|
||||
self.drain_mav()
|
||||
self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
1, # ARM
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED if result else mavutil.mavlink.MAV_RESULT_FAILED,
|
||||
timeout=timeout)
|
||||
if expect_msg is not None:
|
||||
self.wait_statustext(expect_msg, timeout=timeout, the_function=lambda: self.send_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
1, # ARM
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
target_sysid=None,
|
||||
target_compid=None,
|
||||
))
|
||||
|
||||
def arm_vehicle(self, timeout=20):
|
||||
"""Arm vehicle with mavlink arm message."""
|
||||
self.progress("Arm motors with MAVLink cmd")
|
||||
|
Loading…
Reference in New Issue
Block a user