autotest: added initial implementation of flying of ArduPlane

takeoff, circuit, roll, RTL, mission
This commit is contained in:
Andrew Tridgell 2011-11-12 22:13:17 +11:00
parent 0f00b4c722
commit 147dc2529f
6 changed files with 329 additions and 3 deletions

View File

@ -0,0 +1,38 @@
LOG_BITMASK 4095
SWITCH_ENABLE 0
RC2_REV -1
RC1_MAX 2000
RC1_MIN 1000
RC1_TRIM 1500
RC2_MAX 2000
RC2_MIN 1000
RC2_TRIM 1500
RC3_MAX 2000
RC3_MIN 1000
RC3_TRIM 1500
RC4_MAX 2000
RC4_MIN 1000
RC4_TRIM 1500
RC5_MAX 2000
RC5_MIN 1000
RC5_TRIM 1500
RC6_MAX 2000
RC6_MIN 1000
RC6_TRIM 1500
RC7_MAX 2000
RC7_MIN 1000
RC7_TRIM 1500
RC8_MAX 2000
RC8_MIN 1000
RC8_TRIM 1500
FLTMODE1 10
FLTMODE2 11
FLTMODE3 12
FLTMODE4 5
FLTMODE5 2
FLTMODE6 0
FLTMODE_CH 8
PTCH2SRV_P 2
RLL2SRV_I 0.1
RLL2SRV_P 1.2
INVERTEDFLT_CH 7

7
Tools/autotest/ap1.txt Normal file
View File

@ -0,0 +1,7 @@
QGC WPL 110
0 1 0 16 0.000000 0.000000 0.000000 0.000000 -35.362881 149.165222 582.000000 1
1 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 120.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 120.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361721 149.161835 120.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364170 149.164627 120.000000 1
5 0 3 20 0.000000 0.000000 0.000000 0.000000 -35.363289 149.165253 50.000000 1

View File

@ -279,6 +279,7 @@ def fly_ArduCopter(viewerip=None):
failed = False failed = False
e = 'None'
try: try:
mav.wait_heartbeat() mav.wait_heartbeat()
mav.recv_match(type='GPS_RAW', blocking=True) mav.recv_match(type='GPS_RAW', blocking=True)

269
Tools/autotest/arduplane.py Normal file
View File

@ -0,0 +1,269 @@
# 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
else:
cmd = "xvfb-run -s '-screen 0 800x600x24' fgfs --enable-wireframe %s" % fgear_options
fgear = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
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

View File

@ -2,7 +2,8 @@
# APM automatic test suite # APM automatic test suite
# Andrew Tridgell, October 2011 # Andrew Tridgell, October 2011
import pexpect, os, util, sys, shutil, arducopter import pexpect, os, util, sys, shutil
import arducopter, arduplane
import optparse, fnmatch, time, glob, traceback import optparse, fnmatch, time, glob, traceback
os.putenv('TMPDIR', util.reltopdir('tmp')) os.putenv('TMPDIR', util.reltopdir('tmp'))
@ -91,6 +92,7 @@ steps = [
'build.ArduPlane', 'build.ArduPlane',
'defaults.ArduPlane', 'defaults.ArduPlane',
'logs.ArduPlane', 'logs.ArduPlane',
'fly.ArduPlane',
'build1280.ArduCopter', 'build1280.ArduCopter',
'build2560.ArduCopter', 'build2560.ArduCopter',
'build.ArduCopter', 'build.ArduCopter',
@ -147,6 +149,9 @@ def run_step(step):
if step == 'fly.ArduCopter': if step == 'fly.ArduCopter':
return arducopter.fly_ArduCopter() return arducopter.fly_ArduCopter()
if step == 'fly.ArduPlane':
return arduplane.fly_ArduPlane(viewerip=opts.viewerip)
if step == 'convertgpx': if step == 'convertgpx':
return convert_gpx() return convert_gpx()

View File

@ -73,9 +73,15 @@ def pexpect_autoclose(p):
def pexpect_close(p): def pexpect_close(p):
'''close a pexpect child''' '''close a pexpect child'''
global close_list global close_list
try:
p.close() p.close()
except Exception:
pass
time.sleep(1) time.sleep(1)
try:
p.close(force=True) p.close(force=True)
except Exception:
pass
if p in close_list: if p in close_list:
close_list.remove(p) close_list.remove(p)