Tools: expect message for EKF readiness rather than sleeping

This solves a problem for me when running Valgrind as
30s was not sufficient
This commit is contained in:
Peter Barker 2017-01-26 16:53:13 +11:00
parent 1fc0dbe0bd
commit 817efe9b6d
3 changed files with 15 additions and 8 deletions

View File

@ -18,9 +18,12 @@ HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 24
homeloc = None
def arm_rover(mavproxy, mav):
def wait_ready_to_arm(mavproxy):
# wait for EKF and GPS checks to pass
wait_seconds(mav, 30)
mavproxy.expect('IMU0 is using GPS')
def arm_rover(mavproxy, mav):
wait_ready_to_arm(mavproxy);
mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED')

View File

@ -29,6 +29,10 @@ num_wp = 0
speedup_default = 10
def wait_ready_to_arm(mavproxy):
# wait for EKF and GPS checks to pass
mavproxy.expect('IMU0 is using GPS')
def hover(mavproxy, mav, hover_throttle=1500):
mavproxy.send('rc 3 %u\n' % hover_throttle)
return True
@ -1027,8 +1031,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
setup_rc(mavproxy)
homeloc = mav.location()
# wait for EKF and GPS checks to pass
wait_seconds(mav, 30)
wait_ready_to_arm(mavproxy)
# Arm
print("# Arm motors")
@ -1378,8 +1381,7 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
print("Lowering rotor speed")
mavproxy.send('rc 8 1000\n')
# wait for EKF and GPS checks to pass
wait_seconds(mav, 30)
wait_ready_to_arm(mavproxy)
# Arm
print("# Arm motors")

View File

@ -19,12 +19,14 @@ WIND = "0,180,0.2" # speed,direction,variance
homeloc = None
def wait_ready_to_arm(mavproxy):
# wait for EKF and GPS checks to pass
mavproxy.expect('IMU0 is using GPS')
def takeoff(mavproxy, mav):
"""Takeoff get to 30m altitude."""
# wait for EKF and GPS checks to pass
wait_seconds(mav, 30)
wait_ready_to_arm(mavproxy)
mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED')