diff --git a/Tools/autotest/ArduCopter.parm b/Tools/autotest/ArduCopter.parm new file mode 100644 index 0000000000..6088b34a22 --- /dev/null +++ b/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 diff --git a/Tools/autotest/README b/Tools/autotest/README new file mode 100644 index 0000000000..ba3d36dd04 --- /dev/null +++ b/Tools/autotest/README @@ -0,0 +1 @@ +This is an automated test suite for APM diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py new file mode 100644 index 0000000000..434966c98c --- /dev/null +++ b/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) + diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py new file mode 100755 index 0000000000..5769ffcbae --- /dev/null +++ b/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') diff --git a/Tools/autotest/util.py b/Tools/autotest/util.py new file mode 100644 index 0000000000..ee7ff1471a --- /dev/null +++ b/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)