First version of APM autotest

This does SIL testing of ArduPlane and ArduCopter. For now it just does
a basic LOITER test of ArduCopter. It produces logs and a kml of the
flight on apm.tridgell.net

Tools/autotest/ArduCopter.parm

@@ -0,0 +1,30 @@
+RC1_MAX         2000.000000
+RC1_MIN         1000.000000
+RC1_TRIM        1500.000000
+RC2_MAX         2000.000000
+RC2_MIN         1000.000000
+RC2_TRIM        1500.000000
+RC3_MAX         2000.000000
+RC3_MIN         1000.000000
+RC3_TRIM        1500.000000
+RC4_MAX         2000.000000
+RC4_MIN         1000.000000
+RC4_TRIM        1500.000000
+RC5_MAX         2000.000000
+RC5_MIN         1000.000000
+RC5_TRIM        1500.000000
+RC6_MAX         2000.000000
+RC6_MIN         1000.000000
+RC6_TRIM        1500.000000
+RC7_MAX         2000.000000
+RC7_MIN         1000.000000
+RC7_TRIM        1500.000000
+RC8_MAX         2000.000000
+RC8_MIN         1000.000000
+RC8_TRIM        1500.000000
+FLTMODE1	3
+FLTMODE2	1
+FLTMODE3	2
+FLTMODE4	6
+FLTMODE5	5
+FLTMODE6	0

Tools/autotest/README

@@ -0,0 +1 @@
+This is an automated test suite for APM

Tools/autotest/arducopter.py

@@ -0,0 +1,143 @@
+# fly ArduCopter in SIL
+
+import util, pexpect, sys, time, math, shutil
+
+sys.path.insert(0, 'pymavlink')
+import mavutil
+
+HOME_LOCATION='-35.362938,149.165085,650,270'
+
+def arm_motors(mavproxy):
+    '''arm motors'''
+    mavproxy.send('switch 6\n') # stabilize mode
+    mavproxy.expect('STABILIZE>')
+    mavproxy.send('rc 3 1000\n')
+    mavproxy.send('rc 4 2000\n')
+    mavproxy.expect('APM: ARMING MOTORS')
+    mavproxy.send('rc 4 1500\n')
+    print("MOTORS ARMED OK")
+
+def disarm_motors(mavproxy):
+    '''disarm motors'''
+    mavproxy.send('rc 3 1000\n')
+    mavproxy.send('rc 4 1000\n')
+    mavproxy.expect('APM: DISARMING MOTORS')
+    mavproxy.send('rc 4 1500\n')
+    print("MOTORS DISARMED OK")
+    
+
+def takeoff(mavproxy, mav):
+    '''takeoff get to 30m altitude'''
+    mavproxy.send('switch 6\n') # stabilize mode
+    mavproxy.expect('STABILIZE>')
+    mavproxy.send('rc 3 1500\n')
+    mavproxy.send('status\n')
+    m = mav.recv_match(type='VFR_HUD', condition='VFR_HUD.alt>=30', blocking=True)
+    print("Altitude %u" % m.alt)
+    print("TAKEOFF COMPLETE")
+
+
+def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60):
+    '''hold loiter position'''
+    mavproxy.send('switch 5\n') # loiter mode
+    mavproxy.expect('LOITER>')
+    mavproxy.send('status\n')
+    mavproxy.expect('>')
+    m = mav.recv_match(type='VFR_HUD', blocking=True)
+    start_altitude = m.alt
+    tstart = time.time()
+    tholdstart = time.time()
+    print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
+    while time.time() < tstart + timeout:
+        m = mav.recv_match(type='VFR_HUD', blocking=True)
+        print("Altitude %u" % m.alt)
+        if math.fabs(m.alt - start_altitude) > maxaltchange:
+            tholdstart = time.time()
+        if time.time() - tholdstart > holdtime:
+            print("Loiter OK for %u seconds" % holdtime)
+            return True
+    print("Loiter FAILED")
+    return False
+
+
+def land(mavproxy, mav, timeout=60):
+    '''land the quad'''
+    mavproxy.send('switch 6\n')
+    mavproxy.expect('STABILIZE>')
+    mavproxy.send('status\n')
+    mavproxy.expect('>')
+    mavproxy.send('rc 3 1300\n')
+    tstart = time.time()
+    while time.time() < tstart + timeout:
+        m = mav.recv_match(type='VFR_HUD', blocking=True)
+        print("Altitude %u" % m.alt)
+        if m.alt <= 0:
+            print("LANDED OK")
+            return True
+    print("LANDING FAILED")
+    return False
+    
+        
+
+def setup_rc(mavproxy):
+    '''setup RC override control'''
+    for chan in range(1,9):
+        mavproxy.send('rc %u 1500\n' % chan)
+    # zero throttle
+    mavproxy.send('rc 3 1000\n')
+
+
+def fly_ArduCopter():
+    '''fly ArduCopter in SIL'''
+    util.rmfile('eeprom.bin')
+    sil = util.start_SIL('ArduCopter')
+    mavproxy = util.start_MAVProxy_SIL('ArduCopter')
+    mavproxy.expect('Please Run Setup')
+    # we need to restart it after eeprom erase
+    mavproxy.close()
+    sil.close()
+    sil = util.start_SIL('ArduCopter')
+    mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:14550 --quadcopter')
+    mavproxy.expect('Logging to (\S+)')
+    logfile = mavproxy.match.group(1)
+    print("LOGFILE %s" % logfile)
+    mavproxy.expect("Ready to FLY")
+    mavproxy.expect('Received [0-9]+ parameters')
+
+    # setup test parameters
+    mavproxy.send("param load autotest/ArduCopter.parm\n")
+    mavproxy.expect('Loaded [0-9]+ parameters')
+
+    # start hil_quad.py
+    hquad = pexpect.spawn('HILTest/hil_quad.py --fgout=192.168.2.15:9123 --home=%s' % HOME_LOCATION,
+                        logfile=sys.stdout, timeout=10)
+    hquad.expect('Starting at')
+
+    # get a mavlink connection going
+    mav = mavutil.mavlink_connection('127.0.0.1:14550', robust_parsing=True)
+
+    failed = False
+    try:
+        mav.wait_heartbeat()
+        mav.recv_match(type='GPS_RAW')
+        setup_rc(mavproxy)
+        arm_motors(mavproxy)
+        takeoff(mavproxy, mav)
+        loiter(mavproxy, mav)
+        land(mavproxy, mav)
+        disarm_motors(mavproxy)
+    except Exception, 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")
+    
+    if failed:
+        print("FAILED: %s" % e)
+        sys.exit(1)
+        

Tools/autotest/autotest.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+# APM automatic test suite
+# Andrew Tridgell, October 2011
+
+import pexpect, os, util, sys, shutil, arducopter
+import optparse, fnmatch
+
+os.putenv('TMPDIR', util.relcwd('tmp'))
+
+def get_default_params(atype):
+    '''get default parameters'''
+    util.rmfile('eeprom.bin')
+    sil = util.start_SIL(atype)
+    mavproxy = util.start_MAVProxy_SIL(atype)
+    idx = mavproxy.expect(['Please Run Setup', 'Saved [0-9]+ parameters to (\S+)'])
+    if idx == 0:
+        # we need to restart it after eeprom erase
+        mavproxy.close()
+        sil.close()
+        sil = util.start_SIL(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)
+    shutil.copy(parmfile, dest)
+    mavproxy.close()
+    sil.close()
+    print("Saved defaults for %s to %s" % (atype, dest))
+    
+
+############## main program #############
+parser = optparse.OptionParser("autotest")
+parser.add_option("--skip", type='string', default='', help='list of steps to skip (comma separated)')
+parser.add_option("--list", action='store_true', default=False, help='list the available steps')
+
+opts, args = parser.parse_args()
+
+steps = [
+    'build.ArduPlane',
+    'build.ArduCopter',
+    'defaults.ArduPlane',
+    'defaults.ArduCopter',
+    'fly.ArduCopter'
+    ]
+
+skipsteps = opts.skip.split(',')
+
+def skip_step(step):
+    '''see if a step should be skipped'''
+    for skip in skipsteps:
+        if fnmatch.fnmatch(step, skip):
+            return True
+    return False
+
+# kill any previous instance
+util.kill('ArduCopter.elf')
+util.kill('ArduPilot.elf')
+
+for step in steps:
+    if skip_step(step):
+        continue
+    if step == 'build.ArduPlane':
+        util.build_SIL('ArduPlane')
+    elif step == 'build.ArduCopter':
+        util.build_SIL('ArduCopter')
+    elif step == 'defaults.ArduPlane':
+        get_default_params('ArduPlane')
+    elif step == 'defaults.ArduCopter':
+        get_default_params('ArduCopter')
+    elif step == 'fly.ArduCopter':
+        arducopter.fly_ArduCopter()
+    else:
+        raise RuntimeError("Unknown step %s" % step)
+
+util.kill('ArduCopter.elf')
+util.kill('ArduPilot.elf')

Tools/autotest/util.py

@@ -0,0 +1,64 @@
+# utility code for autotest
+
+import os, pexpect, sys
+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 relcwd(path):
+    '''return a path relative to $CWD'''
+    return os.path.join(os.getcwd(), path)
+
+
+def run_cmd(cmd, dir=".", show=False, output=False, checkfail=True):
+    '''run a shell command'''
+    if show:
+        print("Running: '%s' in '%s'" % (cmd, dir))
+    if output:
+        return Popen([cmd], shell=True, stdout=PIPE, cwd=dir).communicate()[0]
+    elif checkfail:
+        return check_call(cmd, shell=True, cwd=dir)
+    else:
+        return call(cmd, shell=True, cwd=dir)
+
+def rmfile(path):
+    '''remove a file if it exists'''
+    try:
+        os.unlink('eeprom.bin')
+    except Exception:
+        pass
+
+def deltree(path):
+    '''delete a tree of files'''
+    run_cmd('rm -rf %s' % path)
+    
+    
+
+def build_SIL(atype):
+    '''build desktop SIL'''
+    run_cmd("make -f ../libraries/Desktop/Makefile.desktop clean hil",
+            dir=relcwd('APM/' + atype),
+            checkfail=True)
+
+def start_SIL(atype):
+    '''launch a SIL instance'''
+    ret = pexpect.spawn('tmp/%s.build/%s.elf' % (atype, atype),
+                        logfile=sys.stdout, timeout=5)
+    ret.expect('Waiting for connection')
+    return ret
+
+def start_MAVProxy_SIL(atype, options=''):
+    '''launch mavproxy connected to a SIL instance'''
+    MAVPROXY = relcwd('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)
+    return ret
+
+
+def kill(name):
+    '''kill a process'''
+    run_cmd('killall -9 -q %s' % name, checkfail=False)