autotest: added a mission to ArduCopter testing

the mission is not flown well at all, but its a start

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

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")
+    shutil.copy(logfile, util.reltopdir("../buildlogs/ArduCopter-test.mavlog"))
-    util.run_cmd("pymavlink/examples/mavtogpx.py buildlogs/ArduCopter-test.mavlog")
+    util.run_cmd(util.reltopdir("../pymavlink/examples/mavtogpx.py") + " " + util.reltopdir("../buildlogs/ArduCopter-test.mavlog"))
-    util.run_cmd("bin/gpxtokml buildlogs/ArduCopter-test.mavlog.gpx")
+    util.run_cmd(util.reltopdir("../bin/gpxtokml") + " " + util.reltopdir("../buildlogs/ArduCopter-test.mavlog.gpx"))
-    
+
     if failed:
         print("FAILED: %s" % e)
         sys.exit(1)

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()

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

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):
+def topdir():
-    '''return a path relative to $HOME'''
+    '''return top of git tree where autotest is running from'''
-    return os.path.join(os.getenv('HOME'), path)
+    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):
+def reltopdir(path):
-    '''return a path relative to $CWD'''
+    '''return a path relative to topdir()'''
-    return os.path.join(os.getcwd(), path)
+    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