AutoTest: add 10sec before starting copter test
We should probably replace this with a check that the EKF has completed it's initialisation and has not gone into constant position mode.
This commit is contained in:
parent
585a105128
commit
74b46818bd
@ -864,6 +864,10 @@ def setup_rc(mavproxy):
|
||||
# zero throttle
|
||||
mavproxy.send('rc 3 1000\n')
|
||||
|
||||
def wait_seconds(mav, wait_time_sec):
|
||||
tstart = time.time()
|
||||
while time.time() < tstart + wait_time_sec:
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
|
||||
def fly_ArduCopter(viewerip=None, map=False):
|
||||
'''fly ArduCopter in SIL
|
||||
@ -946,6 +950,9 @@ def fly_ArduCopter(viewerip=None, map=False):
|
||||
setup_rc(mavproxy)
|
||||
homeloc = mav.location()
|
||||
|
||||
# wait 10sec to allow EKF to settle
|
||||
wait_seconds(mav, 10)
|
||||
|
||||
# Arm
|
||||
print("# Arm motors")
|
||||
if not arm_motors(mavproxy, mav):
|
||||
|
Loading…
Reference in New Issue
Block a user