Browse Source

autotest: fixed failsafe test

increased timeout, added check for home location, and fixed failure
testing on timeout
master
Andrew Tridgell 13 years ago
parent
commit
9700afaf10
  1. 12
      Tools/autotest/arducopter.py

12
Tools/autotest/arducopter.py

@ -193,15 +193,17 @@ def fly_failsafe(mavproxy, mav, side=60):
print("# Enter Failsafe") print("# Enter Failsafe")
mavproxy.send('rc 3 900\n') mavproxy.send('rc 3 900\n')
tstart = time.time() tstart = time.time()
while time.time() < tstart + 120: while time.time() < tstart + 250:
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = current_location(mav) pos = current_location(mav)
#delta = get_distance(start, pos) home_distance = get_distance(HOME, pos)
print("Alt: %u" % m.alt) print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
if(m.alt <= 1): if m.alt <= 1 and home_distance < 10:
print("Reached failsafe home OK")
mavproxy.send('rc 3 1100\n') mavproxy.send('rc 3 1100\n')
return True return True
return True print("Failed to land on failsafe RTL - timed out after 120 seconds")
return False
def fly_simple(mavproxy, mav, side=60, timeout=120): def fly_simple(mavproxy, mav, side=60, timeout=120):

Loading…
Cancel
Save