mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
autotest: added a mission to ArduCopter testing
the mission is not flown well at all, but its a start
This commit is contained in:
Andrew Tridgell
parent
1cdad13474
commit
81cdc19287
@ -29,3 +29,12 @@ FLTMODE3 2
|
||||
FLTMODE4 6
|
||||
FLTMODE5 5
|
||||
FLTMODE6 0
|
||||
NAV_LAT_I 0
|
||||
NAV_LON_I 0
|
||||
NAV_LAT_P 1
|
||||
NAV_LON_P 1
|
||||
STB_PIT_P 2
|
||||
STB_RLL_P 2
|
||||
XTRACK_P 1
|
||||
RATE_PIT_P 0.1
|
||||
RATE_RLL_P 0.1
|
||||
|
||||
@ -5,11 +5,39 @@ import util, pexpect, sys, time, math, shutil, os
|
||||
# get location of scripts
|
||||
testdir=os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
sys.path.insert(0, 'pymavlink')
|
||||
sys.path.insert(0, util.reltopdir('../pymavlink'))
|
||||
import mavutil
|
||||
|
||||
HOME_LOCATION='-35.362938,149.165085,650,270'
|
||||
|
||||
# a list of pexpect objects to read while waiting for
|
||||
# messages. This keeps the output to stdout flowing
|
||||
expect_list = []
|
||||
|
||||
def message_hook(mav, msg):
|
||||
'''called as each mavlink msg is received'''
|
||||
global expect_list
|
||||
if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT' ]:
|
||||
print(msg)
|
||||
for p in expect_list:
|
||||
try:
|
||||
p.read_nonblocking(100, timeout=0)
|
||||
except pexpect.TIMEOUT:
|
||||
pass
|
||||
|
||||
def expect_callback(e):
|
||||
'''called when waiting for a expect pattern'''
|
||||
global expect_list
|
||||
for p in expect_list:
|
||||
if p == e:
|
||||
continue
|
||||
try:
|
||||
while p.read_nonblocking(100, timeout=0):
|
||||
pass
|
||||
except pexpect.TIMEOUT:
|
||||
pass
|
||||
|
||||
|
||||
class location(object):
|
||||
'''represent a GPS coordinate'''
|
||||
def __init__(self, lat, lng, alt=0):
|
||||
@ -128,6 +156,19 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
|
||||
print("Failed to attain distance %u" % distance)
|
||||
return False
|
||||
|
||||
def wait_location(mav, loc, accuracy=5, timeout=30):
|
||||
'''wait for arrival at a location'''
|
||||
tstart = time.time()
|
||||
while time.time() < tstart + timeout:
|
||||
m = mav.recv_match(type='GPS_RAW', blocking=True)
|
||||
pos = current_location(mav)
|
||||
delta = get_distance(loc, pos)
|
||||
print("Distance %.2f meters" % delta)
|
||||
if delta <= accuracy:
|
||||
return True
|
||||
print("Failed to attain location")
|
||||
return False
|
||||
|
||||
|
||||
def fly_square(mavproxy, mav, side=50, timeout=120):
|
||||
'''fly a square, flying N then E'''
|
||||
@ -186,8 +227,20 @@ def land(mavproxy, mav, timeout=60):
|
||||
else:
|
||||
print("LANDING FAILED")
|
||||
return False
|
||||
|
||||
|
||||
|
||||
|
||||
def fly_mission(mavproxy, mav, filename, timeout=120):
|
||||
'''fly a mission from a file'''
|
||||
startloc = current_location(mav)
|
||||
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
|
||||
mavproxy.expect('AUTO>')
|
||||
wait_distance(mav, 30, timeout=120)
|
||||
wait_location(mav, startloc, timeout=300)
|
||||
|
||||
|
||||
def setup_rc(mavproxy):
|
||||
'''setup RC override control'''
|
||||
@ -199,6 +252,8 @@ def setup_rc(mavproxy):
|
||||
|
||||
def fly_ArduCopter():
|
||||
'''fly ArduCopter in SIL'''
|
||||
global expect_list
|
||||
|
||||
util.rmfile('eeprom.bin')
|
||||
sil = util.start_SIL('ArduCopter')
|
||||
mavproxy = util.start_MAVProxy_SIL('ArduCopter')
|
||||
@ -226,14 +281,19 @@ def fly_ArduCopter():
|
||||
mavproxy.expect("Ready to FLY")
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
|
||||
util.expect_setup_callback(mavproxy, expect_callback)
|
||||
|
||||
# start hil_quad.py
|
||||
util.run_cmd('pkill -f hil_quad.py', checkfail=False)
|
||||
hquad = pexpect.spawn('HILTest/hil_quad.py --fgout=192.168.2.15:9123 --home=%s' % HOME_LOCATION,
|
||||
hquad = pexpect.spawn(util.reltopdir('../HILTest/hil_quad.py') + ' --fgout=192.168.2.15:9123 --home=%s' % HOME_LOCATION,
|
||||
logfile=sys.stdout, timeout=10)
|
||||
hquad.expect('Starting at')
|
||||
|
||||
expect_list.extend([hquad, sil, mavproxy])
|
||||
|
||||
# get a mavlink connection going
|
||||
mav = mavutil.mavlink_connection('127.0.0.1:14550', robust_parsing=True)
|
||||
mav.message_hooks.append(message_hook)
|
||||
|
||||
failed = False
|
||||
try:
|
||||
@ -245,18 +305,19 @@ def fly_ArduCopter():
|
||||
fly_square(mavproxy, mav)
|
||||
loiter(mavproxy, mav)
|
||||
land(mavproxy, mav)
|
||||
fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"))
|
||||
disarm_motors(mavproxy)
|
||||
except Exception, e:
|
||||
except pexpect.TIMEOUT, e:
|
||||
failed = True
|
||||
|
||||
mavproxy.close()
|
||||
sil.close()
|
||||
hquad.close()
|
||||
|
||||
shutil.copy(logfile, "buildlogs/ArduCopter-test.mavlog")
|
||||
util.run_cmd("pymavlink/examples/mavtogpx.py buildlogs/ArduCopter-test.mavlog")
|
||||
util.run_cmd("bin/gpxtokml buildlogs/ArduCopter-test.mavlog.gpx")
|
||||
|
||||
shutil.copy(logfile, util.reltopdir("../buildlogs/ArduCopter-test.mavlog"))
|
||||
util.run_cmd(util.reltopdir("../pymavlink/examples/mavtogpx.py") + " " + util.reltopdir("../buildlogs/ArduCopter-test.mavlog"))
|
||||
util.run_cmd(util.reltopdir("../bin/gpxtokml") + " " + util.reltopdir("../buildlogs/ArduCopter-test.mavlog.gpx"))
|
||||
|
||||
if failed:
|
||||
print("FAILED: %s" % e)
|
||||
sys.exit(1)
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
import pexpect, os, util, sys, shutil, arducopter
|
||||
import optparse, fnmatch
|
||||
|
||||
os.putenv('TMPDIR', util.relcwd('tmp'))
|
||||
os.putenv('TMPDIR', util.reltopdir('tmp'))
|
||||
|
||||
def get_default_params(atype):
|
||||
'''get default parameters'''
|
||||
@ -21,7 +21,7 @@ def get_default_params(atype):
|
||||
mavproxy = util.start_MAVProxy_SIL(atype)
|
||||
idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
|
||||
parmfile = mavproxy.match.group(1)
|
||||
dest = os.path.join('buildlogs/%s.defaults.txt' % atype)
|
||||
dest = util.reltopdir('../buildlogs/%s.defaults.txt' % atype)
|
||||
shutil.copy(parmfile, dest)
|
||||
mavproxy.close()
|
||||
sil.close()
|
||||
|
||||
11
Tools/autotest/mission1.txt
Normal file
11
Tools/autotest/mission1.txt
Normal file
@ -0,0 +1,11 @@
|
||||
QGC WPL 110
|
||||
0 1 3 0 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.362324 149.164291 120.000000 1
|
||||
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363670 149.164505 120.000000 1
|
||||
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362358 149.163651 120.000000 1
|
||||
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363777 149.163895 120.000000 1
|
||||
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362411 149.163071 120.000000 1
|
||||
6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363865 149.163223 120.000000 1
|
||||
7 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362431 149.162384 120.000000 1
|
||||
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363968 149.162567 120.000000 1
|
||||
9 0 3 20 0.000000 0.000000 0.000000 0.000000 -35.363228 149.161896 30.000000 1
|
||||
@ -1,16 +1,21 @@
|
||||
# utility code for autotest
|
||||
|
||||
import os, pexpect, sys
|
||||
import os, pexpect, sys, time
|
||||
from subprocess import call, check_call,Popen, PIPE
|
||||
|
||||
|
||||
def relhome(path):
|
||||
'''return a path relative to $HOME'''
|
||||
return os.path.join(os.getenv('HOME'), path)
|
||||
def topdir():
|
||||
'''return top of git tree where autotest is running from'''
|
||||
d = os.path.dirname(os.path.realpath(__file__))
|
||||
assert(os.path.basename(d)=='autotest')
|
||||
d = os.path.dirname(d)
|
||||
assert(os.path.basename(d)=='Tools')
|
||||
d = os.path.dirname(d)
|
||||
return d
|
||||
|
||||
def relcwd(path):
|
||||
'''return a path relative to $CWD'''
|
||||
return os.path.join(os.getcwd(), path)
|
||||
def reltopdir(path):
|
||||
'''return a path relative to topdir()'''
|
||||
return os.path.normpath(os.path.join(topdir(), path))
|
||||
|
||||
|
||||
def run_cmd(cmd, dir=".", show=False, output=False, checkfail=True):
|
||||
@ -40,7 +45,7 @@ def deltree(path):
|
||||
def build_SIL(atype):
|
||||
'''build desktop SIL'''
|
||||
run_cmd("make -f ../libraries/Desktop/Makefile.desktop clean hil",
|
||||
dir=relcwd('APM/' + atype),
|
||||
dir=reltopdir(atype),
|
||||
checkfail=True)
|
||||
|
||||
def start_SIL(atype):
|
||||
@ -52,7 +57,7 @@ def start_SIL(atype):
|
||||
|
||||
def start_MAVProxy_SIL(atype, options=''):
|
||||
'''launch mavproxy connected to a SIL instance'''
|
||||
MAVPROXY = relcwd('MAVProxy/mavproxy.py')
|
||||
MAVPROXY = reltopdir('../MAVProxy/mavproxy.py')
|
||||
ret = pexpect.spawn('%s --master=tcp:127.0.0.1:5760 --aircraft=test.%s %s' % (
|
||||
MAVPROXY,atype,options),
|
||||
logfile=sys.stdout, timeout=60)
|
||||
@ -62,3 +67,22 @@ def start_MAVProxy_SIL(atype, options=''):
|
||||
def kill(name):
|
||||
'''kill a process'''
|
||||
run_cmd('killall -9 -q %s' % name, checkfail=False)
|
||||
|
||||
|
||||
def expect_setup_callback(e, callback):
|
||||
'''setup a callback that is called once a second while waiting for
|
||||
patterns'''
|
||||
def _expect_callback(pattern, timeout=e.timeout):
|
||||
tstart = time.time()
|
||||
while time.time() < tstart + timeout:
|
||||
try:
|
||||
ret = e.expect_saved(pattern, timeout=1)
|
||||
return ret
|
||||
except pexpect.TIMEOUT:
|
||||
e.expect_user_callback(e)
|
||||
pass
|
||||
raise pexpect.TIMEOUT
|
||||
|
||||
e.expect_user_callback = callback
|
||||
e.expect_saved = e.expect
|
||||
e.expect = _expect_callback
|
||||
|
||||
Loading…
Reference in New Issue
Block a user