ardupilot/Tools/autotest/arducopter.py

1332 lines
42 KiB
Python
Raw Normal View History

# fly ArduCopter in SITL
# Flight mode switch positions are set-up in arducopter.param to be
# switch 1 = Circle
# switch 2 = Land
# switch 3 = RTL
# switch 4 = Auto
# switch 5 = Loiter
# switch 6 = Stabilize
import util, pexpect, sys, time, math, shutil, os
from common import *
2013-06-18 03:27:21 -03:00
from pymavlink import mavutil, mavwp
import random
# get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__))
FRAME='+'
TARGET='sitl'
HOME=mavutil.location(-35.362938,149.165085,584,270)
AVCHOME=mavutil.location(40.072842,-105.230575,1586,0)
2011-11-01 20:26:24 -03:00
homeloc = None
2011-11-13 04:24:25 -04:00
num_wp = 0
2011-11-01 20:26:24 -03:00
2013-05-02 23:22:15 -03:00
def hover(mavproxy, mav, hover_throttle=1450):
mavproxy.send('rc 3 %u\n' % hover_throttle)
2012-02-19 16:38:57 -04:00
return True
def calibrate_level(mavproxy, mav):
'''init the accelerometers'''
print("Initialising accelerometers")
mav.calibrate_level()
mavproxy.expect(['APM: action received', 'COMMAND_ACK'])
return True
2011-11-13 06:14:58 -04:00
def arm_motors(mavproxy, mav):
'''arm motors'''
2011-11-09 05:27:36 -04:00
print("Arming motors")
mavproxy.send('switch 6\n') # stabilize mode
2011-11-13 06:20:10 -04:00
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 2000\n')
mavproxy.expect('APM: ARMING MOTORS')
mavproxy.send('rc 4 1500\n')
mav.motors_armed_wait()
print("MOTORS ARMED OK")
2011-11-09 22:12:43 -04:00
return True
2011-11-13 06:14:58 -04:00
def disarm_motors(mavproxy, mav):
'''disarm motors'''
2011-11-09 05:27:36 -04:00
print("Disarming motors")
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 1000\n')
mavproxy.expect('APM: DISARMING MOTORS')
mavproxy.send('rc 4 1500\n')
mav.motors_disarmed_wait()
print("MOTORS DISARMED OK")
2011-11-09 22:12:43 -04:00
return True
2011-11-09 01:43:18 -04:00
2013-05-02 23:22:15 -03:00
def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1700):
'''takeoff get to 30m altitude'''
mavproxy.send('switch 6\n') # stabilize mode
2011-11-13 06:20:10 -04:00
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 %u\n' % takeoff_throttle)
2011-12-16 00:39:05 -04:00
m = mav.recv_match(type='VFR_HUD', blocking=True)
if (m.alt < alt_min):
wait_altitude(mav, alt_min, (alt_min + 5))
2012-02-19 16:38:57 -04:00
hover(mavproxy, mav)
print("TAKEOFF COMPLETE")
2011-11-09 22:12:43 -04:00
return True
2013-05-02 23:22:15 -03:00
# loiter - fly south west, then hold loiter within 5m position and altitude
def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
'''hold loiter position'''
2011-11-12 07:12:38 -04:00
mavproxy.send('switch 5\n') # loiter mode
2011-11-13 06:20:10 -04:00
wait_mode(mav, 'LOITER')
2013-05-02 23:22:15 -03:00
# 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)
start_altitude = m.alt
start = mav.location()
tstart = time.time()
tholdstart = time.time()
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
while time.time() < tstart + holdtime:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location()
2011-12-16 00:39:05 -04:00
delta = get_distance(start, pos)
print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
if math.fabs(m.alt - start_altitude) > maxaltchange:
2013-05-02 23:22:15 -03:00
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:
print("Loiter OK for %u seconds" % holdtime)
return True
print("Loiter FAILED")
return False
def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080):
2011-12-16 00:39:05 -04:00
'''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 %u\n' % climb_throttle)
2011-12-16 00:39:05 -04:00
wait_altitude(mav, alt_min, (alt_min + 5))
else:
print("Lower to alt:%u from %u" % (alt_min, m.alt))
mavproxy.send('rc 3 %u\n' % descend_throttle)
2011-12-16 00:39:05 -04:00
wait_altitude(mav, (alt_min -5), alt_min)
2012-02-19 16:38:57 -04:00
hover(mavproxy, mav)
2011-12-16 00:39:05 -04:00
return True
2013-05-02 23:22:15 -03:00
# fly a square in stabilize mode
def fly_square(mavproxy, mav, side=50, timeout=120):
'''fly a square, flying N then E'''
tstart = time.time()
2011-11-09 22:12:43 -04:00
failed = False
2011-11-13 04:24:25 -04:00
2013-05-02 23:22:15 -03:00
# ensure all sticks in the middle
mavproxy.send('rc 1 1500\n')
mavproxy.send('rc 2 1500\n')
mavproxy.send('rc 3 1500\n')
mavproxy.send('rc 4 1500\n')
2011-11-13 04:24:25 -04:00
2013-05-02 23:22:15 -03:00
# 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')
2013-05-02 23:22:15 -03:00
mav.recv_match(condition='RC_CHANNELS_RAW.chan4_raw==1500', blocking=True)
2013-05-02 23:22:15 -03:00
# 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)
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 2 1300\n')
2011-11-09 22:12:43 -04:00
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 2 1500\n')
2013-05-02 23:22:15 -03:00
# save top left corner of square as waypoint
2011-12-16 00:39:05 -04:00
print("Save WP 3")
2011-11-13 04:24:25 -04:00
save_wp(mavproxy, mav)
2013-05-02 23:22:15 -03:00
# roll right to fly east
print("Going east %u meters" % side)
mavproxy.send('rc 1 1700\n')
2011-11-09 22:12:43 -04:00
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 1 1500\n')
2013-05-02 23:22:15 -03:00
# save top right corner of square as waypoint
2011-12-16 00:39:05 -04:00
print("Save WP 4")
2011-11-13 04:24:25 -04:00
save_wp(mavproxy, mav)
2013-05-02 23:22:15 -03:00
# pitch back to fly south
print("Going south %u meters" % side)
mavproxy.send('rc 2 1700\n')
2011-11-09 22:12:43 -04:00
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 2 1500\n')
2011-11-13 04:24:25 -04:00
2013-05-02 23:22:15 -03:00
# save bottom right corner of square as waypoint
2011-12-16 00:39:05 -04:00
print("Save WP 5")
2011-11-13 04:24:25 -04:00
save_wp(mavproxy, mav)
2013-05-02 23:22:15 -03:00
# roll left to fly west
print("Going west %u meters" % side)
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 1 1300\n')
2011-11-09 22:12:43 -04:00
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 1 1500\n')
2011-11-13 04:24:25 -04:00
2013-05-02 23:22:15 -03:00
# save bottom left corner of square (should be near home) as waypoint
2011-12-16 00:39:05 -04:00
print("Save WP 6")
2011-11-13 04:24:25 -04:00
save_wp(mavproxy, mav)
2011-11-09 22:12:43 -04:00
return not failed
def fly_RTL(mavproxy, mav, side=60, timeout=250):
2013-05-02 23:22:15 -03:00
'''Return, land'''
print("# Enter RTL")
mavproxy.send('switch 3\n')
tstart = time.time()
while time.time() < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location()
2013-05-02 23:22:15 -03:00
home_distance = get_distance(HOME, pos)
print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
if(m.alt <= 1 and home_distance < 10):
return True
2013-05-02 23:22:15 -03:00
return False
def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
'''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')
2013-05-02 23:22:15 -03:00
# switch to stabilize mode
2012-01-06 14:22:13 -04:00
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
2012-02-19 16:38:57 -04:00
hover(mavproxy, mav)
2012-01-06 14:22:13 -04:00
failed = False
2013-05-02 23:22:15 -03:00
# fly east 60 meters
2012-01-06 14:22:13 -04:00
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')
2013-05-02 23:22:15 -03:00
# pull throttle low
2012-01-06 14:22:13 -04:00
print("# Enter Failsafe")
mavproxy.send('rc 3 900\n')
2012-01-06 14:22:13 -04:00
tstart = time.time()
while time.time() < tstart + timeout:
2012-01-06 14:22:13 -04:00
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location()
home_distance = get_distance(HOME, pos)
print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
# check if we've reached home
if m.alt <= 1 and home_distance < 10:
# reduce throttle
2012-01-06 14:22:13 -04:00
mavproxy.send('rc 3 1100\n')
# switch back to stabilize
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
print("Reached failsafe home OK")
2012-01-06 14:22:13 -04:00
return True
print("Failed to land on failsafe RTL - timed out after %u seconds" % timeout)
# reduce throttle
mavproxy.send('rc 3 1100\n')
# switch back to stabilize mode
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
return False
2012-01-06 14:22:13 -04:00
def fly_battery_failsafe(mavproxy, mav, timeout=30):
# assume failure
success = False
# switch to loiter mode so that we hold position
mavproxy.send('switch 5\n')
wait_mode(mav, 'LOITER')
mavproxy.send("rc 3 1500\n")
# enable battery failsafe
mavproxy.send("param set FS_BATT_ENABLE 1\n")
# trigger low voltage
mavproxy.send('param set SIM_BATT_VOLTAGE 10\n')
# wait for LAND mode
if wait_mode(mav, 'LAND', timeout) == 'LAND':
success = True
# disable battery failsafe
mavproxy.send('param set FS_BATT_ENABLE 0\n')
# return status
if success:
print("Successfully entered LAND mode after battery failsafe")
else:
print("Failed to neter LAND mode after battery failsafe")
return success
# fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency
def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# first south
print("turn south")
mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 180):
return False
mavproxy.send('rc 4 1500\n')
#fly west 80m
mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 80):
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)
start_altitude = m.alt
start = mav.location()
tstart = time.time()
tholdstart = time.time()
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
# cut motor 1 to 55% efficiency
print("Cutting motor 1 to 55% efficiency")
mavproxy.send('param set SIM_ENGINE_MUL 0.55\n')
while time.time() < tstart + holdtime:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location()
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() # 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:
print("Stability patch and Loiter OK for %u seconds" % holdtime)
# restore motor 1 to 100% efficiency
mavproxy.send('param set SIM_ENGINE_MUL 1.0\n')
return True
print("Stability Patch FAILED")
# restore motor 1 to 100% efficiency
mavproxy.send('param set SIM_ENGINE_MUL 1.0\n')
return False
2013-05-19 04:19:59 -03:00
# fly_fence_test - fly east until you hit the horizontal circular fence
def fly_fence_test(mavproxy, mav, timeout=180):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# enable fence
mavproxy.send('param set FENCE_ENABLE 1\n')
# first east
print("turn east")
mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 160):
return False
mavproxy.send('rc 4 1500\n')
# fly forward (east) at least 20m
pitching_forward = True
mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 20):
return False
# start timer
tstart = time.time()
while time.time() < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location()
home_distance = get_distance(HOME, pos)
print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
# recenter pitch sticks once we reach home so we don't fly off again
if pitching_forward and home_distance < 10 :
pitching_forward = False
mavproxy.send('rc 2 1500\n')
# disable fence
mavproxy.send('param set FENCE_ENABLE 0\n')
if m.alt <= 1 and home_distance < 10:
# reduce throttle
2013-05-19 04:19:59 -03:00
mavproxy.send('rc 3 1000\n')
# switch mode to stabilize
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
print("Reached home OK")
2013-05-19 04:19:59 -03:00
return True
# disable fence
mavproxy.send('param set FENCE_ENABLE 0\n')
# reduce throttle
mavproxy.send('rc 3 1000\n')
# switch mode to stabilize
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
print("Fence test failed to reach home - timed out after %u seconds" % timeout)
2013-05-19 04:19:59 -03:00
return False
def show_gps_and_sim_positions(mavproxy, on_off):
if on_off == True:
# turn on simulator display of gps and actual position
mavproxy.send('map set showgpspos 1\n')
mavproxy.send('map set showsimpos 1\n')
else:
# turn off simulator display of gps and actual position
mavproxy.send('map set showgpspos 0\n')
mavproxy.send('map set showsimpos 0\n')
# fly_gps_glitch_loiter_test - fly south east in loiter and test reaction to gps glitch
def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=10):
2013-09-19 03:51:18 -03:00
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# turn on simulator display of gps and actual position
show_gps_and_sim_positions(mavproxy, True)
2013-09-19 03:51:18 -03:00
# set-up gps glitch array
glitch_lat = [0.0002996,0.0006958,0.0009431,0.0009991,0.0009444,0.0007716,0.0006221]
glitch_lon = [0.0000717,0.0000912,0.0002761,0.0002626,0.0002807,0.0002049,0.0001304]
glitch_num = len(glitch_lat)
print("GPS Glitches:")
for i in range(1,glitch_num):
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i]))
# turn south east
print("turn south east")
mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 150):
show_gps_and_sim_positions(mavproxy, False)
2013-09-19 03:51:18 -03:00
return False
mavproxy.send('rc 4 1500\n')
# fly forward (south east) at least 60m
mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 60):
show_gps_and_sim_positions(mavproxy, False)
2013-09-19 03:51:18 -03:00
return False
mavproxy.send('rc 2 1500\n')
# wait for copter to slow down
if not wait_groundspeed(mav, 0, 2):
show_gps_and_sim_positions(mavproxy, False)
2013-09-19 03:51:18 -03:00
return False
# record time and position
tstart = time.time()
start_pos = sim_location(mav)
# initialise current glitch
glitch_current = 0;
print("Apply first glitch")
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
# record position for 30 seconds
while time.time() < tstart + timeout:
time_in_sec = int(time.time() - tstart);
if time_in_sec > glitch_current and glitch_current != -1:
glitch_current = time_in_sec
# turn off glitching if we've reached the end of the glitch list
if glitch_current >= glitch_num:
glitch_current = -1
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
else:
#move onto the next glitch
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
m = mav.recv_match(type='VFR_HUD', blocking=True)
curr_pos = sim_location(mav)
moved_distance = get_distance(curr_pos, start_pos)
print("Alt: %u Moved: %.0f" % (m.alt, moved_distance))
if moved_distance > max_distance:
print("Moved over %u meters, Failed!" % max_distance)
# disable gps glitch
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
show_gps_and_sim_positions(mavproxy, False)
2013-09-19 03:51:18 -03:00
return False
# disable gps glitch
if glitch_current != -1:
glitch_current = -1
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
show_gps_and_sim_positions(mavproxy, False)
2013-09-19 03:51:18 -03:00
# if we've gotten this far then we've succeeded
print("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout))
return True
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch
def fly_gps_glitch_auto_test(mavproxy, mav, timeout=30, max_distance=100):
# set-up gps glitch array
glitch_lat = [0.0002996,0.0006958,0.0009431,0.0009991,0.0009444,0.0007716,0.0006221]
glitch_lon = [0.0000717,0.0000912,0.0002761,0.0002626,0.0002807,0.0002049,0.0001304]
glitch_num = len(glitch_lat)
print("GPS Glitches:")
for i in range(1,glitch_num):
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i]))
# Fly mission #1
print("# Upload copter_glitch_mission")
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_glitch_mission.txt")):
print("upload copter_glitch_mission.txt failed")
return False
# this grabs our mission count
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_glitch_mission.txt")):
print("load copter_glitch_mission failed")
return False
# turn on simulator display of gps and actual position
show_gps_and_sim_positions(mavproxy, True)
# load the waypoint count
global homeloc
global num_wp
print("test: Fly a mission from 1 to %u" % num_wp)
mavproxy.send('wp set 1\n')
# switch into AUTO mode and raise throttle
mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO')
mavproxy.send('rc 3 1500\n')
# wait until 100m from home
if not wait_distance(mav, 100, 5, 60):
show_gps_and_sim_positions(mavproxy, False)
return False
# record time and position
tstart = time.time()
start_pos = sim_location(mav)
# initialise current glitch
glitch_current = 0;
print("Apply first glitch")
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
# record position for 30 seconds
while glitch_current < glitch_num:
time_in_sec = int(time.time() - tstart);
if (time_in_sec * 2) > glitch_current and glitch_current != -1:
glitch_current = (time_in_sec * 2)
# apply next glitch
if glitch_current < glitch_num:
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
# turn off glitching
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
# continue with the mission
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500, mode='AUTO')
# wait for arrival back home
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location()
dist_to_home = get_distance(HOME, pos)
while dist_to_home > 5:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location()
dist_to_home = get_distance(HOME, pos)
print("Dist from home: %u" % dist_to_home)
# turn off simulator display of gps and actual position
show_gps_and_sim_positions(mavproxy, False)
print("GPS Glitch test Auto completed: passed=%s" % ret)
return ret
2013-05-02 23:22:15 -03:00
#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=50, timeout=120):
2013-05-02 23:22:15 -03:00
2013-10-05 06:25:17 -03:00
failed = False
# hold position in loiter
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
#set SIMPLE mode for all flight modes
2013-05-02 23:22:15 -03:00
mavproxy.send('param set SIMPLE 63\n')
# switch to stabilize mode
mavproxy.send('switch 6\n')
2011-11-13 06:20:10 -04:00
wait_mode(mav, 'STABILIZE')
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 3 1400\n')
2012-02-19 18:20:51 -04:00
# fly south 50m
print("# Flying south %u meters" % side)
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 1 1300\n')
if not wait_distance(mav, side, 5, 60):
2011-12-16 00:39:05 -04:00
failed = True
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 1 1500\n')
# fly west 8 seconds
print("# Flying west for 8 seconds")
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 2 1300\n')
tstart = time.time()
while time.time() < (tstart + 8):
2011-12-16 00:39:05 -04:00
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (time.time() - tstart)
#print("%u" % delta)
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 2 1500\n')
# fly north 25 meters
print("# Flying north %u meters" % (side/2.0))
2013-05-02 23:22:15 -03:00
mavproxy.send('rc 1 1700\n')
if not wait_distance(mav, side/2, 5, 60):
failed = True
2011-12-16 00:39:05 -04:00
mavproxy.send('rc 1 1500\n')
2011-11-13 04:24:25 -04:00
# fly east 8 seconds
print("# Flying east for 8 seconds")
mavproxy.send('rc 2 1700\n')
2013-05-02 23:22:15 -03:00
tstart = time.time()
while time.time() < (tstart + 8):
2013-05-02 23:22:15 -03:00
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (time.time() - tstart)
#print("%u" % delta)
2011-12-16 00:39:05 -04:00
mavproxy.send('rc 2 1500\n')
2013-05-02 23:22:15 -03:00
2011-12-16 00:39:05 -04:00
#restore to default
mavproxy.send('param set SIMPLE 0\n')
2013-05-02 23:22:15 -03:00
#hover in place
2012-02-19 16:38:57 -04:00
hover(mavproxy, mav)
2011-12-16 00:39:05 -04:00
return not failed
#fly_super_simple - flies a circle around home for 45 seconds
def fly_super_simple(mavproxy, mav, timeout=45):
2013-10-05 06:25:17 -03:00
failed = False
# hold position in loiter
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# fly forward 20m
2013-10-05 06:25:17 -03:00
print("# Flying forward 20 meters")
mavproxy.send('rc 2 1300\n')
if not wait_distance(mav, 20, 5, 60):
failed = True
mavproxy.send('rc 2 1500\n')
#set SUPER SIMPLE mode for all flight modes
mavproxy.send('param set SUPER_SIMPLE 63\n')
# switch to stabilize mode
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1400\n')
# start copter yawing slowly
mavproxy.send('rc 4 1550\n')
# roll left for timeout seconds
print("# rolling left from pilot's point of view for %u seconds" % timeout)
mavproxy.send('rc 1 1300\n')
tstart = time.time()
while time.time() < (tstart + timeout):
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (time.time() - tstart)
# stop rolling and yawing
mavproxy.send('rc 1 1500\n')
mavproxy.send('rc 4 1500\n')
#restore simple mode parameters to default
mavproxy.send('param set SUPER_SIMPLE 0\n')
#hover in place
hover(mavproxy, mav)
return not failed
2013-06-01 06:20:57 -03:00
#fly_circle - flies a circle with 20m radius
def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
2011-12-16 00:39:05 -04:00
# hold position in loiter
2013-06-01 06:20:57 -03:00
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
2013-06-01 06:20:57 -03:00
# face west
print("turn west")
mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 270):
return False
mavproxy.send('rc 4 1500\n')
#set CIRCLE radius
mavproxy.send('param set CIRCLE_RADIUS 3000\n')
2013-06-01 06:20:57 -03:00
# fly forward (east) at least 100m
mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 100):
return False
# return pitch stick back to middle
mavproxy.send('rc 2 1500\n')
2013-06-01 06:20:57 -03:00
# set CIRCLE mode
mavproxy.send('switch 1\n') # circle mode
wait_mode(mav, 'CIRCLE')
2013-06-01 06:20:57 -03:00
# wait
2011-11-13 04:24:25 -04:00
m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt
tstart = time.time()
tholdstart = time.time()
print("Circle at %u meters for %u seconds" % (start_altitude, holdtime))
2013-06-01 09:34:39 -03:00
while time.time() < tstart + holdtime:
2011-11-13 04:24:25 -04:00
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("heading %u" % m.heading)
2011-11-13 04:24:25 -04:00
print("CIRCLE OK for %u seconds" % holdtime)
return True
# fly_auto_test - fly mission which tests a significant number of commands
def fly_auto_test(mavproxy, mav):
# Fly mission #1
print("# Upload copter_glitch_mission")
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_mission.txt")):
print("upload copter_mission.txt failed")
return False
# this grabs our mission count
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_mission.txt")):
print("load copter_mission failed")
return False
# load the waypoint count
global homeloc
global num_wp
print("test: Fly a mission from 1 to %u" % num_wp)
mavproxy.send('wp set 1\n')
# switch into AUTO mode and raise throttle
mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO')
mavproxy.send('rc 3 1500\n')
# fly the mission
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500, mode='AUTO')
# set throttle to minimum
mavproxy.send('rc 3 1000\n')
# wait for disarm
mav.motors_disarmed_wait()
print("MOTORS DISARMED OK")
print("Auto mission completed: passed=%s" % ret)
return ret
# fly_avc_test - fly AVC mission
def fly_avc_test(mavproxy, mav):
# upload mission from file
print("# Upload copter_glitch_mission")
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_AVC2013_mission.txt")):
print("upload copter_mission.txt failed")
return False
# get number of commands in mission
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_AVC2013_mission.txt")):
print("load copter_mission failed")
return False
# load the waypoint count
global homeloc
global num_wp
print("Fly AVC mission from 1 to %u" % num_wp)
mavproxy.send('wp set 1\n')
# switch into AUTO mode and raise throttle
mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO')
mavproxy.send('rc 3 1500\n')
# fly the mission
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500, mode='AUTO')
# set throttle to minimum
mavproxy.send('rc 3 1000\n')
# wait for disarm
mav.motors_disarmed_wait()
print("MOTORS DISARMED OK")
print("AVC mission completed: passed=%s" % ret)
return ret
2013-06-01 06:20:57 -03:00
def land(mavproxy, mav, timeout=60):
'''land the quad'''
print("STARTING LANDING")
mavproxy.send('switch 2\n') # land mode
2013-06-01 06:20:57 -03:00
wait_mode(mav, 'LAND')
print("Entered Landing Mode")
ret = wait_altitude(mav, -5, 1)
print("LANDING: ok= %s" % ret)
return ret
2011-11-13 04:24:25 -04:00
def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file'''
2011-11-01 20:26:24 -03:00
global homeloc
2011-11-13 04:24:25 -04:00
global num_wp
2011-11-28 14:25:16 -04:00
print("test: Fly a mission from 1 to %u" % num_wp)
mavproxy.send('wp set 1\n')
2011-11-13 04:24:25 -04:00
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, mode='AUTO')
2011-11-14 02:56:33 -04:00
print("test: MISSION COMPLETE: passed=%s" % ret)
# wait here until ready
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
return ret
2011-11-13 04:24:25 -04:00
2011-11-14 02:56:33 -04:00
def load_mission_from_file(mavproxy, mav, filename):
2011-11-13 04:24:25 -04:00
'''load a mission from a file'''
global num_wp
2011-11-14 02:56:33 -04:00
wploader = mavwp.MAVWPLoader()
wploader.load(filename)
num_wp = wploader.count()
print("loaded mission with %u waypoints" % num_wp)
return True
def upload_mission_from_file(mavproxy, mav, filename):
'''Upload a mission from a file to APM'''
global num_wp
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received')
mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints')
2011-11-14 02:56:33 -04:00
return True
2011-11-14 02:56:33 -04:00
def save_mission_to_file(mavproxy, mav, filename):
global num_wp
mavproxy.send('wp save %s\n' % filename)
mavproxy.expect('Saved ([0-9]+) waypoints')
num_wp = int(mavproxy.match.group(1))
print("num_wp: %d" % num_wp)
return True
def setup_rc(mavproxy):
'''setup RC override control'''
for chan in range(1,9):
mavproxy.send('rc %u 1500\n' % chan)
# zero throttle
mavproxy.send('rc 3 1000\n')
def fly_ArduCopter(viewerip=None, map=False):
'''fly ArduCopter in SIL
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time
'''
global homeloc
if TARGET != 'sitl':
util.build_SIL('ArduCopter', target=TARGET)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
if viewerip:
sim_cmd += ' --fgout=%s:5503' % viewerip
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/copter_params.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
2011-11-28 14:25:16 -04:00
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
2011-11-09 05:27:36 -04:00
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
2011-11-09 05:27:36 -04:00
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
2011-11-09 05:27:36 -04:00
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
# get a mavlink connection going
2011-11-09 05:27:36 -04:00
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
2011-11-13 04:24:25 -04:00
failed = False
e = 'None'
try:
mav.wait_heartbeat()
setup_rc(mavproxy)
homeloc = mav.location()
2011-12-16 00:39:05 -04:00
print("# Calibrate level")
if not calibrate_level(mavproxy, mav):
2011-12-12 08:08:20 -04:00
print("calibrate_level failed")
failed = True
2013-05-02 23:22:15 -03:00
# Arm
2011-12-16 00:39:05 -04:00
print("# Arm motors")
2011-11-13 06:14:58 -04:00
if not arm_motors(mavproxy, mav):
2011-12-12 08:08:20 -04:00
print("arm_motors failed")
2011-11-09 22:12:43 -04:00
failed = True
2011-11-13 04:24:25 -04:00
2011-12-16 00:39:05 -04:00
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
2013-09-19 03:51:18 -03:00
print("takeoff failed")
failed = True
2013-05-02 23:22:15 -03:00
# Fly a square in Stabilize mode
print("#")
2013-10-05 06:25:17 -03:00
print("########## Fly a square and save WPs with CH7 switch ##########")
2013-05-02 23:22:15 -03:00
print("#")
if not fly_square(mavproxy, mav):
print("fly_square failed")
failed = True
2011-11-13 04:24:25 -04:00
2013-05-02 23:22:15 -03:00
print("# Land")
if not land(mavproxy, mav):
print("land failed")
failed = True
2013-05-02 23:22:15 -03:00
print("Save landing WP")
save_wp(mavproxy, mav)
# save the stored mission to file
print("# Save out the CH7 mission to file")
if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
print("save_mission_to_file failed")
failed = True
# fly the stored mission
print("# Fly CH7 saved mission")
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
print("fly_mission failed")
failed = True
2013-05-02 23:22:15 -03:00
# Throttle Failsafe
print("#")
print("########## Test Failsafe ##########")
print("#")
if not fly_throttle_failsafe(mavproxy, mav):
2012-01-06 14:22:13 -04:00
print("FS failed")
failed = True
2013-05-02 23:22:15 -03:00
# Takeoff
2012-01-06 14:22:13 -04:00
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
# Battery failsafe
if not fly_battery_failsafe(mavproxy, mav):
print("battery failsafe failed")
failed = True
# Takeoff
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
2013-05-19 04:19:59 -03:00
# Stability patch
print("#")
print("########## Test Stability Patch ##########")
print("#")
if not fly_stability_patch(mavproxy, mav, 30):
print("Stability Patch failed")
failed = True
# RTL
print("# RTL #")
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
# Fence test
print("#")
print("########## Test Horizontal Fence ##########")
print("#")
if not fly_fence_test(mavproxy, mav, 180):
print("Fence test failed")
failed = True
# Takeoff
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
# Fly GPS Glitch Loiter test
print("# GPS Glitch Loiter Test")
if not fly_gps_glitch_loiter_test(mavproxy, mav):
print("failed GPS glitch Loiter test")
failed = True
# RTL after GPS Glitch Loiter test
print("# RTL #")
if not fly_RTL(mavproxy, mav):
print("RTL failed")
failed = True
# Fly GPS Glitch test in auto mode
print("# GPS Glitch Auto Test")
if not fly_gps_glitch_auto_test(mavproxy, mav):
print("failed GPS glitch Auto test")
failed = True
# take-off ahead of next test
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
# Loiter for 10 seconds
2013-05-02 23:22:15 -03:00
print("#")
print("########## Test Loiter for 10 seconds ##########")
2013-05-02 23:22:15 -03:00
print("#")
if not loiter(mavproxy, mav):
2011-12-16 00:39:05 -04:00
print("loiter failed")
failed = True
2013-05-02 23:22:15 -03:00
# Loiter Climb
print("#")
print("# Loiter - climb to 30m")
2013-05-02 23:22:15 -03:00
print("#")
if not change_alt(mavproxy, mav, 30):
2011-12-16 00:39:05 -04:00
print("change_alt failed")
failed = True
2013-05-02 23:22:15 -03:00
# Loiter Descend
print("#")
print("# Loiter - descend to 20m")
print("#")
2011-12-16 00:39:05 -04:00
if not change_alt(mavproxy, mav, 20):
print("change_alt failed")
failed = True
2013-05-02 23:22:15 -03:00
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
2011-11-14 02:56:33 -04:00
failed = True
2013-05-02 23:22:15 -03:00
# RTL
print("#")
print("########## Test RTL ##########")
print("#")
if not fly_RTL(mavproxy, mav):
print("RTL failed")
2011-11-09 22:12:43 -04:00
failed = True
2011-11-13 04:24:25 -04:00
2013-05-02 23:22:15 -03:00
# Takeoff
print("# Takeoff")
2011-12-16 00:39:05 -04:00
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
2013-05-02 23:22:15 -03:00
# Simple mode
2011-12-16 00:39:05 -04:00
print("# Fly in SIMPLE mode")
if not fly_simple(mavproxy, mav):
print("fly_simple failed")
failed = True
2013-05-02 23:22:15 -03:00
# RTL
print("#")
print("########## Test RTL ##########")
print("#")
if not fly_RTL(mavproxy, mav):
print("RTL failed")
failed = True
2011-12-16 00:39:05 -04:00
2013-06-01 06:20:57 -03:00
# Takeoff
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
2013-10-05 06:25:17 -03:00
# Fly a circle in super simple mode
print("# Fly a circle in SUPER SIMPLE mode")
if not fly_super_simple(mavproxy, mav):
print("fly super simple failed")
failed = True
# RTL
print("# RTL #")
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
2013-06-01 06:20:57 -03:00
# Circle mode
print("# Fly CIRCLE mode")
if not fly_circle(mavproxy, mav):
print("fly_circle failed")
failed = True
# RTL
print("#")
print("########## Test RTL ##########")
print("#")
if not fly_RTL(mavproxy, mav):
print("RTL failed")
failed = True
print("# Fly copter mission")
if not fly_auto_test(mavproxy, mav):
2011-12-12 08:08:20 -04:00
print("fly_mission failed")
2011-11-13 04:24:25 -04:00
failed = True
else:
print("Flew copter mission OK")
2011-11-13 04:24:25 -04:00
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduCopter-log.bin")):
print("Failed log download")
failed = True
except pexpect.TIMEOUT, e:
failed = True
mav.close()
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
util.pexpect_close(sim)
if os.path.exists('ArduCopter-valgrind.log'):
2011-11-03 20:05:40 -03:00
os.chmod('ArduCopter-valgrind.log', 0644)
shutil.copy("ArduCopter-valgrind.log", util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
if failed:
print("FAILED: %s" % e)
return False
return True
def fly_CopterAVC(viewerip=None, map=False):
'''fly ArduCopter in SIL for AVC2013 mission
'''
global homeloc
if TARGET != 'sitl':
util.build_SIL('ArduCopter', target=TARGET)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
if viewerip:
sim_cmd += ' --fgout=%s:5503' % viewerip
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/copter_AVC2013_params.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
if map:
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
failed = False
e = 'None'
try:
mav.wait_heartbeat()
setup_rc(mavproxy)
homeloc = mav.location()
print("# Calibrate level")
if not calibrate_level(mavproxy, mav):
print("calibrate_level failed")
failed = True
# Arm
print("# Arm motors")
if not arm_motors(mavproxy, mav):
print("arm_motors failed")
failed = True
print("# Fly AVC mission")
if not fly_avc_test(mavproxy, mav):
print("fly_avc_test failed")
failed = True
else:
print("Flew AVC mission OK")
#mission includes disarm at end so should be ok to download logs now
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")):
print("Failed log download")
failed = True
except pexpect.TIMEOUT, e:
failed = True
mav.close()
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
util.pexpect_close(sim)
if failed:
print("FAILED: %s" % e)
return False
return True