mirror of https://github.com/ArduPilot/ardupilot
Added new tests
This commit is contained in:
Jason Short
parent
acbfc14e49
commit
c5859515e3
|
|
@ -45,28 +45,32 @@ def disarm_motors(mavproxy, mav):
|
|||
return True
|
||||
|
||||
|
||||
def takeoff(mavproxy, mav, alt_min = 30, alt_max = 40):
|
||||
def takeoff(mavproxy, mav, alt_min = 30):
|
||||
'''takeoff get to 30m altitude'''
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
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")
|
||||
return True
|
||||
|
||||
|
||||
def loiter(mavproxy, mav, holdtime=20, maxaltchange=10, timeout=60):
|
||||
'''hold loiter position'''
|
||||
mavproxy.send('switch 5\n') # loiter mode
|
||||
wait_mode(mav, 'LOITER')
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
start_altitude = m.alt
|
||||
start = current_location(mav)
|
||||
tstart = time.time()
|
||||
tholdstart = time.time()
|
||||
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
|
||||
while time.time() < tstart + timeout:
|
||||
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:
|
||||
tholdstart = time.time()
|
||||
if time.time() - tholdstart > holdtime:
|
||||
|
|
@ -75,6 +79,20 @@ def loiter(mavproxy, mav, holdtime=20, maxaltchange=10, timeout=60):
|
|||
print("Loiter FAILED")
|
||||
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):
|
||||
'''fly a square, flying N then E'''
|
||||
|
|
@ -83,7 +101,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
|
|||
tstart = time.time()
|
||||
failed = False
|
||||
|
||||
print("Save WP 1")
|
||||
print("Save WP 1 and 2")
|
||||
save_wp(mavproxy, mav)
|
||||
|
||||
print("turn")
|
||||
|
|
@ -99,7 +117,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
|
|||
failed = True
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
|
||||
print("Save WP 2")
|
||||
print("Save WP 3")
|
||||
save_wp(mavproxy, mav)
|
||||
|
||||
print("Going east %u meters" % side)
|
||||
|
|
@ -108,7 +126,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
|
|||
failed = True
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
|
||||
print("Save WP 3")
|
||||
print("Save WP 4")
|
||||
save_wp(mavproxy, mav)
|
||||
|
||||
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')
|
||||
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
|
||||
|
||||
print("Save WP 4")
|
||||
print("Save WP 5")
|
||||
save_wp(mavproxy, mav)
|
||||
|
||||
print("Going west %u meters" % side)
|
||||
|
|
@ -127,30 +145,53 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
|
|||
failed = True
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
|
||||
print("Save WP 5")
|
||||
print("Save WP 6")
|
||||
save_wp(mavproxy, mav)
|
||||
|
||||
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):
|
||||
'''land the quad'''
|
||||
print("STARTING LANDING")
|
||||
mavproxy.send('switch 6\n')
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
|
||||
# start by dropping throttle till we have lost 5m
|
||||
mavproxy.send('rc 3 1380\n')
|
||||
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)
|
||||
mavproxy.send('switch 2\n')
|
||||
wait_mode(mav, 'LAND')
|
||||
print("Entered Landing Mode")
|
||||
ret = wait_altitude(mav, -5, 5)
|
||||
print("LANDING: ok= %s" % 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
|
||||
wait_mode(mav, 'AUTO')
|
||||
#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)
|
||||
# wait here until ready
|
||||
mavproxy.send('switch 5\n')
|
||||
|
|
@ -308,47 +349,81 @@ def fly_ArduCopter(viewerip=None):
|
|||
setup_rc(mavproxy)
|
||||
homeloc = current_location(mav)
|
||||
|
||||
print("# Calibrate level")
|
||||
if not calibrate_level(mavproxy, mav):
|
||||
print("calibrate_level failed")
|
||||
failed = True
|
||||
|
||||
print("# Arm motors")
|
||||
if not arm_motors(mavproxy, mav):
|
||||
print("arm_motors failed")
|
||||
failed = True
|
||||
|
||||
if not takeoff(mavproxy, mav, 10, 15):
|
||||
print("# Takeoff")
|
||||
if not takeoff(mavproxy, mav, 10):
|
||||
print("takeoff failed")
|
||||
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")
|
||||
if not fly_square(mavproxy, mav):
|
||||
print("fly_square failed")
|
||||
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
|
||||
print("# Save out the C7 mission")
|
||||
if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
|
||||
print("save_mission_to_file failed")
|
||||
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
|
||||
print("# Fly CH 7 saved mission")
|
||||
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
||||
print("fly_mission failed")
|
||||
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")
|
||||
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
|
||||
print("upload_mission_from_file failed")
|
||||
|
|
@ -392,3 +467,10 @@ def fly_ArduCopter(viewerip=None):
|
|||
print("FAILED: %s" % e)
|
||||
return False
|
||||
return True
|
||||
|
||||
#Fly a circle for 60 seconds
|
||||
#print("# Fly a Circle")
|
||||
#if not circle(mavproxy, mav):
|
||||
# print("circle failed")
|
||||
# failed = True
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue