Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AutoTest: reorganise copter tests

Browse Source
This commit is contained in:
Randy Mackay 2013-05-03 11:22:15 +09:00
parent 44b0c790f3
commit f14284e7a0
2 changed files with 210 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

310
Tools/autotest/arducopter.py
View File

@ -14,7 +14,7 @@ HOME=mavutil.location(-35.362938,149.165085,584,270)
homeloc = None homeloc = None
num_wp = 0 num_wp = 0
def hover(mavproxy, mav, hover_throttle=1300): def hover(mavproxy, mav, hover_throttle=1450):
mavproxy.send('rc 3 %u\n' % hover_throttle) mavproxy.send('rc 3 %u\n' % hover_throttle)
return True return True
@ -49,7 +49,7 @@ def disarm_motors(mavproxy, mav):
return True return True
def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1510): def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1700):
'''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')
@ -61,10 +61,29 @@ def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1510):
print("TAKEOFF COMPLETE") print("TAKEOFF COMPLETE")
return True return True
def loiter(mavproxy, mav, holdtime=60, maxaltchange=20): # loiter - fly south west, then hold loiter within 5m position and altitude
def loiter(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=5):
'''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')
# first aim south east
print("turn south east")
mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 170):
return False
mavproxy.send('rc 4 1500\n')
#fly south east 50m
mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 50):
return False
mavproxy.send('rc 2 1500\n')
# wait for copter to slow moving
if not wait_groundspeed(mav, 0, 2):
return False
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 = mav.location() start = mav.location()
@ -77,7 +96,9 @@ def loiter(mavproxy, mav, holdtime=60, maxaltchange=20):
delta = get_distance(start, pos) delta = get_distance(start, pos)
print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) 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() # this will cause this test to timeout and fails
if delta > maxdistchange:
tholdstart = time.time() # this will cause this test to timeout and fails
if time.time() - tholdstart > holdtime: if time.time() - tholdstart > holdtime:
print("Loiter OK for %u seconds" % holdtime) print("Loiter OK for %u seconds" % holdtime)
return True return True
@ -98,101 +119,127 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108
hover(mavproxy, mav) hover(mavproxy, mav)
return True return True
# fly a square in stabilize mode
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'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
tstart = time.time() tstart = time.time()
failed = False failed = False
print("Save WP 1 and 2") # ensure all sticks in the middle
save_wp(mavproxy, mav) mavproxy.send('rc 1 1500\n')
mavproxy.send('rc 2 1500\n')
print("turn") mavproxy.send('rc 3 1500\n')
hover(mavproxy, mav)
mavproxy.send('rc 4 1550\n')
if not wait_heading(mav, 0):
return False
mavproxy.send('rc 4 1500\n') mavproxy.send('rc 4 1500\n')
# switch to loiter mode temporarily to stop us from rising
mavproxy.send('switch 5\n')
wait_mode(mav, 'LOITER')
# first aim north
print("turn right towards north")
mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 10):
return False
mavproxy.send('rc 4 1500\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan4_raw==1500', blocking=True)
# save bottom left corner of box as waypoint
print("Save WP 1 & 2")
save_wp(mavproxy, mav)
# switch back to stabilize mode
mavproxy.send('rc 3 1400\n')
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
# pitch forward to fly north
print("Going north %u meters" % side) print("Going north %u meters" % side)
mavproxy.send('rc 2 1390\n') mavproxy.send('rc 2 1300\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True failed = True
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# save top left corner of square as waypoint
print("Save WP 3") print("Save WP 3")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
# roll right to fly east
print("Going east %u meters" % side) print("Going east %u meters" % side)
mavproxy.send('rc 1 1700\n') mavproxy.send('rc 1 1700\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True failed = True
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
# save top right corner of square as waypoint
print("Save WP 4") print("Save WP 4")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
# pitch back to fly south
print("Going south %u meters" % side) print("Going south %u meters" % side)
mavproxy.send('rc 2 1700\n') mavproxy.send('rc 2 1700\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True failed = True
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
# save bottom right corner of square as waypoint
print("Save WP 5") print("Save WP 5")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
# roll left to fly west
print("Going west %u meters" % side) print("Going west %u meters" % side)
mavproxy.send('rc 1 1390\n') mavproxy.send('rc 1 1300\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True failed = True
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
# save bottom left corner of square (should be near home) as waypoint
print("Save WP 6") print("Save WP 6")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
return not failed return not failed
def fly_RTL(mavproxy, mav, side=60, timeout=250): def fly_RTL(mavproxy, mav, side=60, timeout=250):
'''Fly, return, land''' '''Return, land'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
hover(mavproxy, mav)
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("# Enter RTL") print("# Enter RTL")
mavproxy.send('switch 3\n') mavproxy.send('switch 3\n')
tstart = time.time() tstart = time.time()
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)
pos = mav.location() pos = mav.location()
#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):
return True return True
return True return False
def fly_failsafe(mavproxy, mav, side=60, timeout=180): def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
'''Fly, Failsafe, return, land''' '''Fly east, Failsafe, return, land'''
# switch to loiter mode temporarily to stop us from rising
mavproxy.send('switch 5\n')
wait_mode(mav, 'LOITER')
# first aim east
print("turn east")
mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 135):
return False
mavproxy.send('rc 4 1500\n')
# switch to stabilize mode
mavproxy.send('switch 6\n') mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
hover(mavproxy, mav) hover(mavproxy, mav)
failed = False failed = False
# fly east 60 meters
print("# Going forward %u meters" % side) print("# Going forward %u meters" % side)
mavproxy.send('rc 2 1350\n') mavproxy.send('rc 2 1350\n')
if not wait_distance(mav, side, 5, 60): if not wait_distance(mav, side, 5, 60):
failed = True failed = True
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# pull throttle low
print("# Enter Failsafe") print("# Enter Failsafe")
mavproxy.send('rc 3 900\n') mavproxy.send('rc 3 900\n')
tstart = time.time() tstart = time.time()
@ -216,44 +263,63 @@ def fly_failsafe(mavproxy, mav, side=60, timeout=180):
mavproxy.send('switch 6\n') mavproxy.send('switch 6\n')
return False return False
#fly_simple - assumes the simple bearing is initialised to be directly north
# flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south
def fly_simple(mavproxy, mav, side=100, timeout=120):
#set SIMPLE mode
mavproxy.send('param set SIMPLE 63\n')
def fly_simple(mavproxy, mav, side=60, timeout=120):
'''fly Simple, flying N then E''' '''fly Simple, flying N then E'''
mavproxy.send('switch 6\n') mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1350\n') mavproxy.send('rc 3 1400\n')
tstart = time.time() tstart = time.time()
failed = False failed = False
print("# Going forward %u meters" % side) # fly west 100m
mavproxy.send('rc 2 1390\n') print("# Flying west %u meters" % side)
if not wait_distance(mav, side, 10, 60): mavproxy.send('rc 1 1300\n')
if not wait_distance(mav, side, 5, 60):
failed = True failed = True
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 1 1500\n')
print("# Going east for 30 seconds") # fly north 15 seconds
mavproxy.send('rc 1 1700\n') print("# Flying north for 15 seconds")
mavproxy.send('rc 2 1300\n')
tstart = time.time() tstart = time.time()
while time.time() < (tstart + 30): while time.time() < (tstart + 15):
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (time.time() - tstart) delta = (time.time() - tstart)
#print("%u" % delta) #print("%u" % delta)
mavproxy.send('rc 2 1500\n')
# fly east 50 meters
print("# Flying east %u meters" % (side/2.0))
mavproxy.send('rc 1 1700\n')
if not wait_distance(mav, side/2, 5, 60):
failed = True
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
print("# Going back %u meters" % side) # fly south 15 seconds
print("# Flying south for 15 seconds")
mavproxy.send('rc 2 1700\n') mavproxy.send('rc 2 1700\n')
if not wait_distance(mav, side, 10, 60): tstart = time.time()
failed = True while time.time() < (tstart + 15):
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (time.time() - tstart)
#print("%u" % delta)
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
#restore to default #restore to default
mavproxy.send('param set SIMPLE 0\n') mavproxy.send('param set SIMPLE 0\n')
#hover in place
hover(mavproxy, mav) hover(mavproxy, mav)
return not failed 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")
@ -421,6 +487,7 @@ def fly_ArduCopter(viewerip=None, map=False):
print("calibrate_level failed") print("calibrate_level failed")
failed = True failed = True
# Arm
print("# Arm motors") print("# Arm motors")
if not arm_motors(mavproxy, mav): if not arm_motors(mavproxy, mav):
print("arm_motors failed") print("arm_motors failed")
@ -430,50 +497,11 @@ def fly_ArduCopter(viewerip=None, map=False):
if not takeoff(mavproxy, mav, 10): if not takeoff(mavproxy, mav, 10):
print("takeoff failed") print("takeoff failed")
failed = True failed = True
print("# Test RTL") # Fly a square in Stabilize mode
if not fly_RTL(mavproxy, mav): print("#")
print("RTL failed") print("########## Fly A square and save WPs with CH7 switch ##########")
failed = True print("#")
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
print("# Test Failsafe")
if not fly_failsafe(mavproxy, mav):
print("FS failed")
failed = True
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
# Loiter for 30 seconds
print("# Loiter for 45 seconds")
if not loiter(mavproxy, mav, 45):
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")
if not fly_square(mavproxy, mav): if not fly_square(mavproxy, mav):
print("fly_square failed") print("fly_square failed")
failed = True failed = True
@ -485,59 +513,115 @@ def fly_ArduCopter(viewerip=None, map=False):
print("Save landing WP") print("Save landing WP")
save_wp(mavproxy, mav) save_wp(mavproxy, mav)
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
# save the stored mission # save the stored mission to file
print("# Save out the C7 mission") print("# Save out the CH7 mission to file")
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
# save the stored mission # fly the stored mission
print("# Fly CH 7 saved mission") print("# Fly CH7 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 # Throttle Failsafe
mavproxy.send('param set SIMPLE 63\n') print("#")
print("########## Test Failsafe ##########")
print("#")
if not fly_throttle_failsafe(mavproxy, mav):
print("FS failed")
failed = True
# Takeoff
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
# Loiter for 30 seconds
print("#")
print("########## Test Loiter for 40 seconds ##########")
print("#")
if not loiter(mavproxy, mav, 30):
print("loiter failed")
failed = True
# Loiter Climb
print("#")
print("# Loiter - climb to 60m")
print("#")
if not change_alt(mavproxy, mav, 60):
print("change_alt failed")
failed = True
# Loiter Descend
print("#")
print("# Loiter - descend to 20m")
print("#")
if not change_alt(mavproxy, mav, 20):
print("change_alt failed")
failed = True
if not takeoff(mavproxy, mav, 10): if not takeoff(mavproxy, mav, 10):
print("takeoff failed") print("takeoff failed")
failed = True failed = True
# RTL
print("#")
print("########## Test RTL ##########")
print("#")
if not fly_RTL(mavproxy, mav):
print("RTL failed")
failed = True
# Takeoff
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
# Simple mode
print("# Fly in SIMPLE mode") print("# Fly in SIMPLE mode")
if not fly_simple(mavproxy, mav): if not fly_simple(mavproxy, mav):
print("fly_simple failed") print("fly_simple failed")
failed = True failed = True
# RTL
print("#")
print("########## Test RTL ##########")
print("#")
if not fly_RTL(mavproxy, mav):
print("RTL failed")
failed = True
# Fly mission #1
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, "mission1.txt")):
print("upload_mission_from_file failed") print("upload_mission_from_file failed")
failed = True failed = True
# this grabs our mission count # this grabs our mission count
print("# store mission1 locally") print("# store mission1 locally")
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")): if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission1.txt")):
print("load_mission_from_file failed") print("load_mission_from_file failed")
failed = True failed = True
print("# Fly mission 2") print("# Fly mission 1")
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
else: else:
print("Flew mission2 OK") print("Flew mission 1 OK")
print("# Land") #mission includes LAND at end so should be ok to disamr
if not land(mavproxy, mav): print("# disarm motors")
print("land failed") if not disarm_motors(mavproxy, mav):
print("disarm_motors failed")
failed = True failed = True
#print("# disarm motors")
#if not disarm_motors(mavproxy, mav):
# print("disarm_motors failed")
# failed = True
except pexpect.TIMEOUT, e: except pexpect.TIMEOUT, e:
failed = True failed = True
@ -554,7 +638,3 @@ def fly_ArduCopter(viewerip=None, map=False):
print("FAILED: %s" % e) print("FAILED: %s" % e)
return False return False
return True return True

25
Tools/autotest/mission1.txt
View File

@ -1,11 +1,16 @@
QGC WPL 110 QGC WPL 110
0 1 3 0 0.000000 0.000000 0.000000 0.000000 -35.362881 149.165222 582.000000 1 0 1 0 16 0 0 0 0 -35.362881 149.165222 582.000000 1
1 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362324 149.164291 120.000000 1 1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.362881 149.165222 20.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363670 149.164505 120.000000 1 2 0 3 16 0.000000 3.000000 0.000000 0.000000 -35.364652 149.163501 20.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362358 149.163651 120.000000 1 3 0 3 115 640.000000 20.000000 1.000000 1.000000 0.000000 0.000000 0.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363777 149.163895 120.000000 1 4 0 3 19 35.000000 0.000000 0.000000 1.000000 0.000000 0.000000 20.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362411 149.163071 120.000000 1 5 0 3 16 0.000000 3.000000 0.000000 0.000000 -35.365361 149.163501 20.000000 1
6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363865 149.163223 120.000000 1 6 0 3 16 1.000000 0.000000 0.000000 0.000000 -35.365361 149.163995 40.000000 1
7 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362431 149.162384 120.000000 1 7 0 3 16 0.000000 3.000000 0.000000 0.000000 -35.365361 149.164563 20.000000 1
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363968 149.162567 120.000000 1 8 0 3 114 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
9 0 3 20 0.000000 0.000000 0.000000 0.000000 -35.363228 149.161896 30.000000 1 9 0 3 113 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 40.000000 1
10 0 3 16 0.000000 3.000000 0.000000 0.000000 -35.364652 149.164531 20.000000 1
11 0 3 16 0.000000 3.000000 0.000000 0.000000 -35.364652 149.163995 20.000000 1
12 0 3 177 11.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
13 0 3 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 20.000000 1
14 0 3 21 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1