Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: added a lot more interal checking 

check each of the sub-tests pass for overall pass
This commit is contained in:
Andrew Tridgell 2012-02-14 11:19:31 +11:00
parent 273a974814
commit c105645c57
2 changed files with 56 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
Tools/autotest/arduplane.py
View File

@ -42,7 +42,8 @@ def takeoff(mavproxy, mav):
mavproxy.send('rc 3 1800\n') mavproxy.send('rc 3 1800\n')
# gain a bit of altitude # gain a bit of altitude
wait_altitude(mav, homeloc.alt+30, homeloc.alt+60, timeout=30) if not wait_altitude(mav, homeloc.alt+30, homeloc.alt+60, timeout=30):
return False
# level off # level off
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
@ -55,7 +56,8 @@ def fly_left_circuit(mavproxy, mav):
mavproxy.send('switch 4\n') mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA') wait_mode(mav, 'FBWA')
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
wait_level_flight(mavproxy, mav) if not wait_level_flight(mavproxy, mav):
return False
print("Flying left circuit") print("Flying left circuit")
# do 4 turns # do 4 turns
@ -63,10 +65,12 @@ def fly_left_circuit(mavproxy, mav):
# hard left # hard left
print("Starting turn %u" % i) print("Starting turn %u" % i)
mavproxy.send('rc 1 1000\n') mavproxy.send('rc 1 1000\n')
wait_heading(mav, 270 - (90*i)) if not wait_heading(mav, 270 - (90*i)):
return False
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
print("Starting leg %u" % i) print("Starting leg %u" % i)
wait_distance(mav, 100) if not wait_distance(mav, 100):
return False
print("Circuit complete") print("Circuit complete")
return True return True
@ -75,31 +79,40 @@ def fly_RTL(mavproxy, mav):
print("Flying home in RTL") print("Flying home in RTL")
mavproxy.send('switch 2\n') mavproxy.send('switch 2\n')
wait_mode(mav, 'RTL') wait_mode(mav, 'RTL')
wait_location(mav, homeloc, accuracy=90, if not wait_location(mav, homeloc, accuracy=90,
target_altitude=100, height_accuracy=20) target_altitude=homeloc.alt+100, height_accuracy=20,
timeout=90):
return False
print("RTL Complete") print("RTL Complete")
return True return True
def fly_LOITER(mavproxy, mav): def fly_LOITER(mavproxy, mav, num_circles=4):
'''loiter where we are''' '''loiter where we are'''
print("Testing LOITER") print("Testing LOITER for %u turns" % num_circles)
mavproxy.send('switch 3\n') mavproxy.send('switch 3\n')
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
while True: while num_circles > 0:
wait_heading(mav, 0) if not wait_heading(mav, 0):
wait_heading(mav, 180) return False
if not wait_heading(mav, 180):
return False
num_circles -= 1
print("Loiter %u circles left" % num_circles)
print("Completed Loiter OK")
return True return True
def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30): def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
'''wait for level flight''' '''wait for level flight'''
tstart = time.time() tstart = time.time()
print("Waiting for level flight")
while time.time() < tstart + timeout: while time.time() < tstart + timeout:
m = mav.recv_match(type='ATTITUDE', blocking=True) m = mav.recv_match(type='ATTITUDE', blocking=True)
roll = math.degrees(m.roll) roll = math.degrees(m.roll)
pitch = math.degrees(m.pitch) pitch = math.degrees(m.pitch)
print("Roll=%.1f Pitch=%.1f" % (roll, pitch)) print("Roll=%.1f Pitch=%.1f" % (roll, pitch))
if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy: if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy:
print("Attained level flight")
return True return True
print("Failed to attain level flight") print("Failed to attain level flight")
return False return False
@ -114,7 +127,8 @@ def change_altitude(mavproxy, mav, altitude, accuracy=10):
mavproxy.send('rc 2 2000\n') mavproxy.send('rc 2 2000\n')
else: else:
mavproxy.send('rc 2 1000\n') mavproxy.send('rc 2 1000\n')
wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2) if not wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2):
return False
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
print("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt) print("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt)
return wait_level_flight(mavproxy, mav) return wait_level_flight(mavproxy, mav)
@ -124,7 +138,8 @@ def axial_left_roll(mavproxy, mav, count=1):
'''fly a left axial roll''' '''fly a left axial roll'''
# full throttle! # full throttle!
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
change_altitude(mavproxy, mav, homeloc.alt+200) if not change_altitude(mavproxy, mav, homeloc.alt+200):
return False
# fly the roll in manual # fly the roll in manual
mavproxy.send('switch 6\n') mavproxy.send('switch 6\n')
@ -133,9 +148,12 @@ def axial_left_roll(mavproxy, mav, count=1):
while count > 0: while count > 0:
print("Starting roll") print("Starting roll")
mavproxy.send('rc 1 1000\n') mavproxy.send('rc 1 1000\n')
wait_roll(mav, -150, accuracy=20) if not wait_roll(mav, -150, accuracy=20):
wait_roll(mav, 150, accuracy=20) return False
wait_roll(mav, 0, accuracy=20) if not wait_roll(mav, 150, accuracy=20):
return False
if not wait_roll(mav, 0, accuracy=20):
return False
count -= 1 count -= 1
# back to FBWA # back to FBWA
@ -150,7 +168,8 @@ def inside_loop(mavproxy, mav, count=1):
'''fly a inside loop''' '''fly a inside loop'''
# full throttle! # full throttle!
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
change_altitude(mavproxy, mav, homeloc.alt+200) if not change_altitude(mavproxy, mav, homeloc.alt+200):
return False
# fly the loop in manual # fly the loop in manual
mavproxy.send('switch 6\n') mavproxy.send('switch 6\n')
@ -159,8 +178,10 @@ def inside_loop(mavproxy, mav, count=1):
while count > 0: while count > 0:
print("Starting loop") print("Starting loop")
mavproxy.send('rc 2 1000\n') mavproxy.send('rc 2 1000\n')
wait_pitch(mav, 80, accuracy=20) if not wait_pitch(mav, 80, accuracy=20):
wait_pitch(mav, 0, accuracy=20) return False
if not wait_pitch(mav, 0, accuracy=20):
return False
count -= 1 count -= 1
# back to FBWA # back to FBWA
@ -183,6 +204,7 @@ def setup_rc(mavproxy):
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None): def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file''' '''fly a mission from a file'''
global homeloc global homeloc
print("Flying mission %s" % filename)
mavproxy.send('wp load %s\n' % filename) mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received') mavproxy.expect('flight plan received')
mavproxy.send('wp list\n') mavproxy.send('wp list\n')
@ -287,6 +309,9 @@ def fly_ArduPlane(viewerip=None):
if not fly_RTL(mavproxy, mav): if not fly_RTL(mavproxy, mav):
print("Failed RTL") print("Failed RTL")
failed = True failed = True
if not fly_LOITER(mavproxy, mav):
print("Failed LOITER")
failed = True
if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10, if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10,
target_altitude=homeloc.alt+100): target_altitude=homeloc.alt+100):
print("Failed mission") print("Failed mission")

16
Tools/autotest/common.py
View File

@ -84,6 +84,7 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
if abs(climb_rate) > 0: if abs(climb_rate) > 0:
tstart = time.time(); tstart = time.time();
if m.alt >= alt_min and m.alt <= alt_max: if m.alt >= alt_min and m.alt <= alt_max:
print("Altitude OK")
return True return True
print("Failed to attain altitude range") print("Failed to attain altitude range")
return False return False
@ -110,6 +111,7 @@ def wait_roll(mav, roll, accuracy, timeout=30):
r = math.degrees(m.roll) r = math.degrees(m.roll)
print("Roll %u" % r) print("Roll %u" % r)
if math.fabs(r - roll) <= accuracy: if math.fabs(r - roll) <= accuracy:
print("Attained roll %u" % roll)
return True return True
print("Failed to attain roll %u" % roll) print("Failed to attain roll %u" % roll)
return False return False
@ -123,6 +125,7 @@ def wait_pitch(mav, pitch, accuracy, timeout=30):
r = math.degrees(m.pitch) r = math.degrees(m.pitch)
print("Pitch %u" % r) print("Pitch %u" % r)
if math.fabs(r - pitch) <= accuracy: if math.fabs(r - pitch) <= accuracy:
print("Attained pitch %u" % pitch)
return True return True
print("Failed to attain pitch %u" % pitch) print("Failed to attain pitch %u" % pitch)
return False return False
@ -136,6 +139,7 @@ def wait_heading(mav, heading, accuracy=5, timeout=30):
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Heading %u" % m.heading) print("Heading %u" % m.heading)
if math.fabs(m.heading - heading) <= accuracy: if math.fabs(m.heading - heading) <= accuracy:
print("Attained heading %u" % heading)
return True return True
print("Failed to attain heading %u" % heading) print("Failed to attain heading %u" % heading)
return False return False
@ -151,8 +155,10 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
delta = get_distance(start, pos) delta = get_distance(start, pos)
print("Distance %.2f meters" % delta) print("Distance %.2f meters" % delta)
if math.fabs(delta - distance) <= accuracy: if math.fabs(delta - distance) <= accuracy:
print("Attained distance %.2f meters OK" % delta)
return True return True
if(delta > (distance + accuracy)): if delta > (distance + accuracy):
print("Failed distance - overshoot delta=%f distance=%f" % (delta, distance))
return False return False
print("Failed to attain distance %u" % distance) print("Failed to attain distance %u" % distance)
return False return False
@ -163,11 +169,13 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height
tstart = time.time() tstart = time.time()
if target_altitude is None: if target_altitude is None:
target_altitude = loc.alt target_altitude = loc.alt
print("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % (
loc.lat, loc.lng, target_altitude, height_accuracy))
while time.time() < tstart + timeout: while time.time() < tstart + timeout:
m = mav.recv_match(type='GPS_RAW', blocking=True) m = mav.recv_match(type='GPS_RAW', blocking=True)
pos = current_location(mav) pos = current_location(mav)
delta = get_distance(loc, pos) delta = get_distance(loc, pos)
print("Distance %.2f meters" % delta) print("Distance %.2f meters alt %.1f" % (delta, pos.alt))
if delta <= accuracy: if delta <= accuracy:
if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy: if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy:
continue continue
@ -212,9 +220,9 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
print("Reached final waypoint %u" % seq) print("Reached final waypoint %u" % seq)
return True return True
if seq > current_wp+1: if seq > current_wp+1:
print("Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1)) print("Failed: Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1))
return False return False
print("Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) print("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end))
return False return False
def save_wp(mavproxy, mav): def save_wp(mavproxy, mav):