mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Tools: autotest: fix race conditions in arm_vehicle and disarm_vehicle
This commit is contained in:
parent
ba3cfbfb54
commit
da2e00dd6f
@ -923,6 +923,7 @@ class AutoTest(ABC):
|
|||||||
def arm_vehicle(self, timeout=20):
|
def arm_vehicle(self, timeout=20):
|
||||||
"""Arm vehicle with mavlink arm message."""
|
"""Arm vehicle with mavlink arm message."""
|
||||||
self.progress("Arm motors with MAVLink cmd")
|
self.progress("Arm motors with MAVLink cmd")
|
||||||
|
tstart = self.get_sim_time()
|
||||||
self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||||
1, # ARM
|
1, # ARM
|
||||||
0,
|
0,
|
||||||
@ -932,7 +933,6 @@ class AutoTest(ABC):
|
|||||||
0,
|
0,
|
||||||
0,
|
0,
|
||||||
timeout=timeout)
|
timeout=timeout)
|
||||||
tstart = self.get_sim_time()
|
|
||||||
while self.get_sim_time_cached() - tstart < timeout:
|
while self.get_sim_time_cached() - tstart < timeout:
|
||||||
self.wait_heartbeat()
|
self.wait_heartbeat()
|
||||||
if self.mav.motors_armed():
|
if self.mav.motors_armed():
|
||||||
@ -946,6 +946,7 @@ class AutoTest(ABC):
|
|||||||
p2 = 0
|
p2 = 0
|
||||||
if force:
|
if force:
|
||||||
p2 = 21196 # magic force disarm value
|
p2 = 21196 # magic force disarm value
|
||||||
|
tstart = self.get_sim_time()
|
||||||
self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||||
0, # DISARM
|
0, # DISARM
|
||||||
p2,
|
p2,
|
||||||
@ -955,7 +956,6 @@ class AutoTest(ABC):
|
|||||||
0,
|
0,
|
||||||
0,
|
0,
|
||||||
timeout=timeout)
|
timeout=timeout)
|
||||||
tstart = self.get_sim_time()
|
|
||||||
while self.get_sim_time_cached() - tstart < timeout:
|
while self.get_sim_time_cached() - tstart < timeout:
|
||||||
self.wait_heartbeat()
|
self.wait_heartbeat()
|
||||||
if not self.mav.motors_armed():
|
if not self.mav.motors_armed():
|
||||||
|
Loading…
Reference in New Issue
Block a user