ardupilot/Tools/autotest/arduplane.py

273 lines
8.0 KiB
Python
Raw Normal View History

# fly ArduPlane in SIL
import util, pexpect, sys, time, math, shutil, os
from common import *
# get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__))
sys.path.insert(0, util.reltopdir('../pymavlink'))
import mavutil
HOME_LOCATION='-35.362938,149.165085,584,270'
homeloc = None
def takeoff(mavproxy, mav):
'''takeoff get to 30m altitude'''
mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA')
# some rudder to counteract the prop torque
mavproxy.send('rc 4 1600\n')
# get it moving a bit first to avoid bad fgear ground physics
mavproxy.send('rc 3 1150\n')
mav.recv_match(condition='VFR_HUD.groundspeed>3', blocking=True)
# a bit faster
mavproxy.send('rc 3 1600\n')
mav.recv_match(condition='VFR_HUD.groundspeed>10', blocking=True)
# hit the gas harder now, and give it some elevator
mavproxy.send('rc 4 1500\n')
mavproxy.send('rc 2 1200\n')
mavproxy.send('rc 3 1800\n')
# gain a bit of altitude
wait_altitude(mav, homeloc.alt+30, homeloc.alt+40, timeout=30)
# level off
mavproxy.send('rc 2 1500\n')
print("TAKEOFF COMPLETE")
return True
def fly_left_circuit(mavproxy, mav):
'''fly a left circuit, 200m on a side'''
mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA')
print("Flying left circuit")
# do 4 turns
for i in range(0,4):
# hard left
print("Starting turn %u" % i)
mavproxy.send('rc 1 1000\n')
wait_heading(mav, 270 - (90*i))
mavproxy.send('rc 1 1500\n')
print("Starting leg %u" % i)
wait_distance(mav, 100)
print("Circuit complete")
return True
def fly_RTL(mavproxy, mav):
'''fly to home'''
mavproxy.send('switch 2\n')
wait_mode(mav, 'RTL')
wait_location(mav, homeloc, accuracy=30,
target_altitude=100, height_accuracy=10)
print("RTL Complete")
return True
def fly_LOITER(mavproxy, mav):
'''loiter where we are'''
mavproxy.send('switch 3\n')
wait_mode(mav, 'LOITER')
while True:
wait_heading(mav, 0)
wait_heading(mav, 180)
return True
def change_altitude(mavproxy, mav, altitude, accuracy=10):
'''get to a given altitude'''
mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA')
alt_error = mav.messages['VFR_HUD'].alt - altitude
if alt_error > 0:
mavproxy.send('rc 2 2000\n')
else:
mavproxy.send('rc 2 1000\n')
wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2)
mavproxy.send('rc 2 1500\n')
print("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt)
return True
def axial_left_roll(mavproxy, mav, count=1):
'''fly a left axial roll'''
# full throttle!
mavproxy.send('rc 3 2000\n')
change_altitude(mavproxy, mav, homeloc.alt+200)
# fly the roll in manual
mavproxy.send('switch 5\n')
wait_mode(mav, 'STABILIZE')
while count > 0:
print("Starting roll")
mavproxy.send('rc 1 1000\n')
wait_roll(mav, -150, accuracy=20)
wait_roll(mav, 150, accuracy=20)
wait_roll(mav, 0, accuracy=20)
count -= 1
# back to FBWA
mavproxy.send('rc 1 1500\n')
mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA')
mavproxy.send('rc 3 1700\n')
if not wait_distance(mav, 50):
return False
return True
def setup_rc(mavproxy):
'''setup RC override control'''
for chan in [1,2,4,5,6,7]:
mavproxy.send('rc %u 1500\n' % chan)
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 8 1800\n')
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file'''
global homeloc
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received')
mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints')
mavproxy.send('switch 1\n') # auto mode
wait_mode(mav, 'AUTO')
if not wait_distance(mav, 30, timeout=120):
return False
if not wait_location(mav, homeloc, accuracy=50, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy):
return False
print("Mission OK")
return True
def fly_ArduPlane(viewerip=None):
'''fly ArduPlane 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 expect_list, homeloc
sil = util.start_SIL('ArduPlane', wipe=True)
options = '--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduPlane-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/ArduPlane.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
util.expect_setup_callback(mavproxy, expect_callback)
fgear_options = '''
--generic=socket,in,25,,5502,udp,MAVLink \
--generic=socket,out,50,,5501,udp,MAVLink \
--aircraft=Rascal110-JSBSim \
--control=mouse \
--disable-intro-music \
--airport=YKRY \
--lat=-35.362851 \
--lon=149.165223 \
--heading=350 \
--altitude=0 \
--geometry=650x550 \
--jpg-httpd=5502 \
--disable-anti-alias-hud \
--disable-hud-3d \
--disable-enhanced-lighting \
--disable-distance-attenuation \
--disable-horizon-effect \
--shading-flat \
--disable-textures \
--timeofday=noon \
--fdm=jsb \
--disable-sound \
--disable-fullscreen \
--disable-random-objects \
--disable-ai-models \
--shading-flat \
--fog-disable \
--disable-specular-highlight \
--disable-skyblend \
--fg-scenery="/home/tridge/project/UAV/fgdata/Scenery" \
--disable-anti-alias-hud \
--wind=0@0 \
'''
# start fgear
if os.getenv('DISPLAY'):
cmd = 'fgfs %s' % fgear_options
fgear = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
else:
cmd = "xvfb-run --server-num=42 -s '-screen 0 800x600x24' fgfs --enable-wireframe %s" % fgear_options
util.kill_xvfb(42)
fgear = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
fgear.xvfb_server_num = 42
util.pexpect_autoclose(fgear)
expect_list.extend([fgear, sil, mavproxy])
# 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)
failed = False
e = 'None'
try:
mav.wait_heartbeat()
setup_rc(mavproxy)
mav.recv_match(type='GPS_RAW', condition='GPS_RAW.lat != 0 and GPS_RAW.alt != 0 and VFR_HUD.alt != 0',
blocking=True)
homeloc = current_location(mav)
print("Home location: %s" % homeloc)
if not takeoff(mavproxy, mav):
failed = True
if not fly_left_circuit(mavproxy, mav):
failed = True
if not axial_left_roll(mavproxy, mav, 1):
failed = True
if not fly_RTL(mavproxy, mav):
failed = True
if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10,
target_altitude=homeloc.alt+100):
failed = True
except pexpect.TIMEOUT, e:
failed = True
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
util.pexpect_close(fgear)
if os.path.exists('ArduPlane-valgrind.log'):
os.chmod('ArduPlane-valgrind.log', 0644)
shutil.copy("ArduPlane-valgrind.log", util.reltopdir("../buildlogs/ArduPlane-valgrind.log"))
if failed:
print("FAILED: %s" % e)
return False
return True