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

Added new tests

This commit is contained in:
Jason Short 2011-12-15 20:39:05 -08:00
parent acbfc14e49
commit c5859515e3
1 changed files with 119 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
Tools/autotest/arducopter.py
View File

@ -45,28 +45,32 @@ def disarm_motors(mavproxy, mav):
return True return True
def takeoff(mavproxy, mav, alt_min = 30, alt_max = 40): def takeoff(mavproxy, mav, alt_min = 30):
'''takeoff get to 30m altitude''' '''takeoff get to 30m altitude'''
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1500\n') mavproxy.send('rc 3 1500\n')
wait_altitude(mav, alt_min, alt_max) m = mav.recv_match(type='VFR_HUD', blocking=True)
if (m.alt < alt_min):
wait_altitude(mav, alt_min, (alt_min + 5))
print("TAKEOFF COMPLETE") print("TAKEOFF COMPLETE")
return True return True
def loiter(mavproxy, mav, holdtime=20, maxaltchange=10, timeout=60): def loiter(mavproxy, mav, holdtime=20, maxaltchange=10, timeout=60):
'''hold loiter position''' '''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt start_altitude = m.alt
start = current_location(mav)
tstart = time.time() tstart = time.time()
tholdstart = time.time() tholdstart = time.time()
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
while time.time() < tstart + timeout: while time.time() < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Altitude %u" % m.alt) pos = current_location(mav)
delta = get_distance(start, pos)
print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
if math.fabs(m.alt - start_altitude) > maxaltchange: if math.fabs(m.alt - start_altitude) > maxaltchange:
tholdstart = time.time() tholdstart = time.time()
if time.time() - tholdstart > holdtime: if time.time() - tholdstart > holdtime:
@ -75,6 +79,20 @@ def loiter(mavproxy, mav, holdtime=20, maxaltchange=10, timeout=60):
print("Loiter FAILED") print("Loiter FAILED")
return False return False
def change_alt(mavproxy, mav, alt_min):
'''change altitude'''
m = mav.recv_match(type='VFR_HUD', blocking=True)
if(m.alt < alt_min):
print("Rise to alt:%u from %u" % (alt_min, m.alt))
mavproxy.send('rc 3 1800\n')
wait_altitude(mav, alt_min, (alt_min + 5))
else:
print("Lower to alt:%u from %u" % (alt_min, m.alt))
mavproxy.send('rc 3 1100\n')
wait_altitude(mav, (alt_min -5), alt_min)
mavproxy.send('rc 3 1430\n')
return True
def fly_square(mavproxy, mav, side=50, timeout=120): def fly_square(mavproxy, mav, side=50, timeout=120):
'''fly a square, flying N then E''' '''fly a square, flying N then E'''
@ -83,7 +101,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
tstart = time.time() tstart = time.time()
failed = False failed = False
print("Save WP 1") print("Save WP 1 and 2")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
print("turn") print("turn")
@ -99,7 +117,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
failed = True failed = True
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
print("Save WP 2") print("Save WP 3")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
print("Going east %u meters" % side) print("Going east %u meters" % side)
@ -108,7 +126,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
failed = True failed = True
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
print("Save WP 3") print("Save WP 4")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
print("Going south %u meters" % side) print("Going south %u meters" % side)
@ -118,7 +136,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
print("Save WP 4") print("Save WP 5")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
print("Going west %u meters" % side) print("Going west %u meters" % side)
@ -127,31 +145,54 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
failed = True failed = True
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
print("Save WP 5") print("Save WP 6")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
return not failed return not failed
def fly_simple(mavproxy, mav, side=60, timeout=120):
'''fly a square, flying N then E'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1430\ n')
tstart = time.time()
failed = False
print("# Going forward %u meters" % side)
mavproxy.send('rc 2 1350\n')
if not wait_distance(mav, side, 5, 60):
failed = True
mavproxy.send('rc 2 1500\n')
print("# Going east for 30 seconds")
mavproxy.send('rc 1 1650\n')
tstart = time.time()
while time.time() < (tstart + 30):
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (time.time() - tstart)
#print("%u" % delta)
mavproxy.send('rc 1 1500\n')
print("# Going back %u meters" % side)
mavproxy.send('rc 2 1650\n')
if not wait_distance(mav, side, 5, 60):
failed = True
mavproxy.send('rc 2 1500\n')
#restore to default
mavproxy.send('param set SIMPLE 0\n')
return not failed
def land(mavproxy, mav, timeout=60): def land(mavproxy, mav, timeout=60):
'''land the quad''' '''land the quad'''
print("STARTING LANDING") print("STARTING LANDING")
mavproxy.send('switch 6\n') mavproxy.send('switch 2\n')
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'LAND')
print("Entered Landing Mode")
# start by dropping throttle till we have lost 5m ret = wait_altitude(mav, -5, 5)
mavproxy.send('rc 3 1380\n') print("LANDING: ok= %s" % ret)
m = mav.recv_match(type='VFR_HUD', blocking=True)
wait_altitude(mav, 0, m.alt-5)
# now let it settle gently
mavproxy.send('rc 3 1400\n')
tstart = time.time()
ret = wait_altitude(mav, -5, 0)
print("LANDING: ok=%s" % ret)
return ret return ret
@ -182,7 +223,7 @@ def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
mavproxy.send('switch 4\n') # auto mode mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO') wait_mode(mav, 'AUTO')
#wait_altitude(mav, 30, 40) #wait_altitude(mav, 30, 40)
ret = wait_waypoint(mav, 0, num_wp, timeout=500) ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
print("test: MISSION COMPLETE: passed=%s" % ret) print("test: MISSION COMPLETE: passed=%s" % ret)
# wait here until ready # wait here until ready
mavproxy.send('switch 5\n') mavproxy.send('switch 5\n')
@ -308,47 +349,81 @@ def fly_ArduCopter(viewerip=None):
setup_rc(mavproxy) setup_rc(mavproxy)
homeloc = current_location(mav) homeloc = current_location(mav)
print("# Calibrate level")
if not calibrate_level(mavproxy, mav): if not calibrate_level(mavproxy, mav):
print("calibrate_level failed") print("calibrate_level failed")
failed = True failed = True
print("# Arm motors")
if not arm_motors(mavproxy, mav): if not arm_motors(mavproxy, mav):
print("arm_motors failed") print("arm_motors failed")
failed = True failed = True
if not takeoff(mavproxy, mav, 10, 15): print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed") print("takeoff failed")
failed = True failed = True
# Loiter for 10 seconds
print("# Loiter for 30 seconds")
if not loiter(mavproxy, mav, 10):
print("loiter failed")
failed = True
print("# Change alt to 60m")
if not change_alt(mavproxy, mav, 60):
print("change_alt failed")
failed = True
print("# Change alt to 20m")
if not change_alt(mavproxy, mav, 20):
print("change_alt failed")
failed = True
print("# Change alt to 20m")
if not change_alt(mavproxy, mav, 20):
print("change_alt failed")
failed = True
print("# Fly A square") print("# Fly A square")
if not fly_square(mavproxy, mav): if not fly_square(mavproxy, mav):
print("fly_square failed") print("fly_square failed")
failed = True failed = True
print("# Land")
if not land(mavproxy, mav):
print("land failed")
failed = True
print("Save landing WP")
save_wp(mavproxy, mav)
# save the stored mission # save the stored mission
print("# Save out the C7 mission") print("# Save out the C7 mission")
if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")): if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
print("save_mission_to_file failed") print("save_mission_to_file failed")
failed = True failed = True
# Loiter for 10 seconds
print("# Loiter for 20 seconds")
if not loiter(mavproxy, mav, 20):
print("loiter failed")
failed = True
#Fly a circle for 60 seconds
print("# Fly a Circle")
if not circle(mavproxy, mav):
print("circle failed")
failed = True
# save the stored mission # save the stored mission
print("# Fly CH 7 saved mission") print("# Fly CH 7 saved mission")
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10): if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
print("fly_mission failed") print("fly_mission failed")
failed = True failed = True
#set SIMPLE mode
mavproxy.send('param set SIMPLE 63\n')
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
print("# Fly in SIMPLE mode")
if not fly_simple(mavproxy, mav):
print("fly_simple failed")
failed = True
print("# Upload mission1") print("# Upload mission1")
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")): if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
print("upload_mission_from_file failed") print("upload_mission_from_file failed")
@ -392,3 +467,10 @@ def fly_ArduCopter(viewerip=None):
print("FAILED: %s" % e) print("FAILED: %s" % e)
return False return False
return True return True
#Fly a circle for 60 seconds
#print("# Fly a Circle")
#if not circle(mavproxy, mav):
# print("circle failed")
# failed = True