mirror of https://github.com/ArduPilot/ardupilot
autotest: added initial implementation of flying of ArduPlane
takeoff, circuit, roll, RTL, mission
This commit is contained in:
Andrew Tridgell
parent
0f00b4c722
commit
147dc2529f
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -279,6 +279,7 @@ def fly_ArduCopter(viewerip=None):
|
|||
|
||||
|
||||
failed = False
|
||||
e = 'None'
|
||||
try:
|
||||
mav.wait_heartbeat()
|
||||
mav.recv_match(type='GPS_RAW', blocking=True)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -2,7 +2,8 @@
|
|||
# APM automatic test suite
|
||||
# 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
|
||||
|
||||
os.putenv('TMPDIR', util.reltopdir('tmp'))
|
||||
|
|
@ -91,6 +92,7 @@ steps = [
|
|||
'build.ArduPlane',
|
||||
'defaults.ArduPlane',
|
||||
'logs.ArduPlane',
|
||||
'fly.ArduPlane',
|
||||
'build1280.ArduCopter',
|
||||
'build2560.ArduCopter',
|
||||
'build.ArduCopter',
|
||||
|
|
@ -147,6 +149,9 @@ def run_step(step):
|
|||
if step == 'fly.ArduCopter':
|
||||
return arducopter.fly_ArduCopter()
|
||||
|
||||
if step == 'fly.ArduPlane':
|
||||
return arduplane.fly_ArduPlane(viewerip=opts.viewerip)
|
||||
|
||||
if step == 'convertgpx':
|
||||
return convert_gpx()
|
||||
|
||||
|
|
|
|||
|
|
@ -73,9 +73,15 @@ def pexpect_autoclose(p):
|
|||
def pexpect_close(p):
|
||||
'''close a pexpect child'''
|
||||
global close_list
|
||||
try:
|
||||
p.close()
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(1)
|
||||
try:
|
||||
p.close(force=True)
|
||||
except Exception:
|
||||
pass
|
||||
if p in close_list:
|
||||
close_list.remove(p)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue