From 7a0d39c197b0571376558e81ec81ea1b606aa73c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Nov 2011 22:31:01 +1100 Subject: [PATCH] autotest: added a mission to ArduCopter testing the mission is not flown well at all, but its a start --- Tools/autotest/ArduCopter.parm | 9 ++++ Tools/autotest/arducopter.py | 79 ++++++++++++++++++++++++++++++---- Tools/autotest/autotest.py | 4 +- Tools/autotest/mission1.txt | 11 +++++ Tools/autotest/util.py | 42 ++++++++++++++---- 5 files changed, 125 insertions(+), 20 deletions(-) create mode 100644 Tools/autotest/mission1.txt diff --git a/Tools/autotest/ArduCopter.parm b/Tools/autotest/ArduCopter.parm index 5e9a2a45be..3465fc56d2 100644 --- a/Tools/autotest/ArduCopter.parm +++ b/Tools/autotest/ArduCopter.parm @@ -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 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 53969cd0d9..a98c8bf337 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 5769ffcbae..de645e2952 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -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() diff --git a/Tools/autotest/mission1.txt b/Tools/autotest/mission1.txt new file mode 100644 index 0000000000..7162f6a4c7 --- /dev/null +++ b/Tools/autotest/mission1.txt @@ -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 diff --git a/Tools/autotest/util.py b/Tools/autotest/util.py index ee7ff1471a..df6fd981be 100644 --- a/Tools/autotest/util.py +++ b/Tools/autotest/util.py @@ -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