diff --git a/Tools/autotest/Rover.parm b/Tools/autotest/Rover.parm new file mode 100644 index 0000000000..09a38dce29 --- /dev/null +++ b/Tools/autotest/Rover.parm @@ -0,0 +1,14 @@ +LOG_BITMASK 4095 +MAG_ENABLE 1 +TRIM_ARSPD_CM 1000 +TRIM_THROTTLE 100 +THR_MAX 100 +RC3_MAX 2000 +RC3_MIN 1000 +RC3_TRIM 1500 +FLTMODE1 0 +FLTMODE2 0 +FLTMODE3 11 +FLTMODE4 10 +FLTMODE5 2 +FLTMODE6 0 diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py new file mode 100644 index 0000000000..60101e9ab0 --- /dev/null +++ b/Tools/autotest/apmrover2.py @@ -0,0 +1,170 @@ +# drive APMrover2 in SITL + +import util, pexpect, sys, time, math, shutil, os +from common import * +import mavutil, random + +# get location of scripts +testdir=os.path.dirname(os.path.realpath(__file__)) + + +HOME=mavutil.location(-35.362938,149.165085,584,270) + +homeloc = None + +def drive_left_circuit(mavproxy, mav): + '''drive a left circuit, 50m on a side''' + mavproxy.send('switch 6\n') + wait_mode(mav, 'MANUAL') + mavproxy.send('rc 3 2000\n') + + print("Driving left circuit") + # do 4 turns + for i in range(0,4): + # hard left + print("Starting turn %u" % i) + mavproxy.send('rc 1 1000\n') + if not wait_heading(mav, 270 - (90*i), accuracy=10): + return False + mavproxy.send('rc 1 1500\n') + print("Starting leg %u" % i) + if not wait_distance(mav, 50, accuracy=7): + return False + print("Circuit complete") + return True + +def drive_RTL(mavproxy, mav): + '''drive to home''' + print("Driving home in RTL") + mavproxy.send('switch 3\n') + wait_mode(mav, 'RTL') + if not wait_location(mav, homeloc, accuracy=22, timeout=90): + return False + print("RTL Complete") + return True + +def setup_rc(mavproxy): + '''setup RC override control''' + for chan in [1,2,3,4,5,6,7]: + mavproxy.send('rc %u 1500\n' % chan) + mavproxy.send('rc 8 1800\n') + + +def drive_mission(mavproxy, mav, filename): + '''drive a mission from a file''' + global homeloc + print("Driving mission %s" % filename) + 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 4\n') # auto mode + mavproxy.send('rc 3 1500\n') + wait_mode(mav, 'AUTO') + if not wait_waypoint(mav, 1, 4, max_dist=5): + return False + wait_mode(mav, 'MANUAL') + print("Mission OK") + return True + + +def drive_APMrover2(viewerip=None): + '''drive APMrover2 in SIL + + you can pass viewerip as an IP address to optionally send fg and + mavproxy packets too for local viewing of the mission in real time + ''' + global homeloc + + options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' + if viewerip: + options += " --out=%s:14550" % viewerip + + sil = util.start_SIL('APMrover2', wipe=True) + mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options) + mavproxy.expect('Received [0-9]+ parameters') + + # setup test parameters + mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200)) + mavproxy.send("param load %s/Rover.parm\n" % testdir) + mavproxy.expect('Loaded [0-9]+ parameters') + + # restart with new parms + util.pexpect_close(mavproxy) + util.pexpect_close(sil) + + sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % ( + HOME.lat, HOME.lng, HOME.alt, HOME.heading) + + runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10) + runsim.delaybeforesend = 0 + util.pexpect_autoclose(runsim) + runsim.expect('Starting at lat') + + sil = util.start_SIL('APMrover2') + mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options) + mavproxy.expect('Logging to (\S+)') + logfile = mavproxy.match.group(1) + print("LOGFILE %s" % logfile) + + buildlog = util.reltopdir("../buildlogs/APMrover2-test.mavlog") + print("buildlog=%s" % buildlog) + if os.path.exists(buildlog): + os.unlink(buildlog) + os.link(logfile, buildlog) + + mavproxy.expect('Received [0-9]+ parameters') + + util.expect_setup_callback(mavproxy, expect_callback) + + expect_list_clear() + expect_list_extend([runsim, sil, mavproxy]) + + print("Started simulator") + + # get a mavlink connection going + try: + mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) + except Exception, msg: + print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + raise + mav.message_hooks.append(message_hook) + mav.idle_hooks.append(idle_hook) + + failed = False + e = 'None' + try: + print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) + mav.wait_heartbeat() + print("Setting up RC parameters") + setup_rc(mavproxy) + print("Waiting for GPS fix") + mav.wait_gps_fix() + homeloc = mav.location() + print("Home location: %s" % homeloc) + if not drive_left_circuit(mavproxy, mav): + print("Failed left circuit") + failed = True + if not drive_RTL(mavproxy, mav): + print("Failed RTL") + failed = True + if not drive_mission(mavproxy, mav, os.path.join(testdir, "rover1.txt")): + print("Failed mission") + failed = True + except pexpect.TIMEOUT, e: + print("Failed with timeout") + failed = True + + mav.close() + util.pexpect_close(mavproxy) + util.pexpect_close(sil) + util.pexpect_close(runsim) + + if os.path.exists('APMrover2-valgrind.log'): + os.chmod('APMrover2-valgrind.log', 0644) + shutil.copy("APMrover2-valgrind.log", util.reltopdir("../buildlogs/APMrover2-valgrind.log")) + + if failed: + print("FAILED: %s" % e) + return False + return True diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 4425f0d153..7e3067ef95 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -119,6 +119,8 @@ def alarm_handler(signum, frame): results.addfile('ArduPlane defaults', 'ArduPlane.defaults.txt') results.addfile('ArduCopter build log', 'ArduCopter.txt') results.addfile('ArduCopter defaults', 'ArduCopter.defaults.txt') + results.addfile('APMrover2 build log', 'APMrover2.txt') + results.addfile('APMrover2 defaults', 'APMrover2.defaults.txt') write_webresults(results) os.killpg(0, signal.SIGKILL) except Exception: @@ -135,23 +137,33 @@ parser.add_option("--timeout", default=2400, type='int', help='maximum runtime i opts, args = parser.parse_args() -import arducopter, arduplane +import arducopter, arduplane, apmrover2 steps = [ 'prerequesites', - 'build1280.ArduPlane', - 'build2560.ArduPlane', 'build.All', 'build.Examples', + + 'build1280.ArduPlane', + 'build2560.ArduPlane', 'build.ArduPlane', 'defaults.ArduPlane', 'fly.ArduPlane', 'logs.ArduPlane', + + 'build1280.APMrover2', + 'build2560.APMrover2', + 'build.APMrover2', + 'defaults.APMrover2', + 'drive.APMrover2', + 'logs.APMrover2', + 'build2560.ArduCopter', 'build.ArduCopter', 'defaults.ArduCopter', 'fly.ArduCopter', 'logs.ArduCopter', + 'convertgpx', ] @@ -176,6 +188,9 @@ def run_step(step): if step == 'build.ArduPlane': return util.build_SIL('ArduPlane') + if step == 'build.APMrover2': + return util.build_SIL('APMrover2') + if step == 'build.ArduCopter': return util.build_SIL('ArduCopter') @@ -191,24 +206,39 @@ def run_step(step): if step == 'build2560.ArduPlane': return util.build_AVR('ArduPlane', board='mega2560') + if step == 'build1280.APMrover2': + return util.build_AVR('APMrover2', board='mega') + + if step == 'build2560.APMrover2': + return util.build_AVR('APMrover2', board='mega2560') + if step == 'defaults.ArduPlane': return get_default_params('ArduPlane') if step == 'defaults.ArduCopter': return get_default_params('ArduCopter') + if step == 'defaults.APMrover2': + return get_default_params('APMrover2') + if step == 'logs.ArduPlane': return dump_logs('ArduPlane') if step == 'logs.ArduCopter': return dump_logs('ArduCopter') + if step == 'logs.APMrover2': + return dump_logs('APMrover2') + if step == 'fly.ArduCopter': return arducopter.fly_ArduCopter(viewerip=opts.viewerip) if step == 'fly.ArduPlane': return arduplane.fly_ArduPlane(viewerip=opts.viewerip) + if step == 'drive.APMrover2': + return apmrover2.drive_APMrover2(viewerip=opts.viewerip) + if step == 'build.All': return build_all() @@ -320,6 +350,10 @@ def run_tests(steps): results.addfile('ArduCopter code size', 'ArduCopter.sizes.txt') results.addfile('ArduCopter stack sizes', 'ArduCopter.framesizes.txt') results.addfile('ArduCopter defaults', 'ArduCopter.defaults.txt') + results.addfile('APMrover2 build log', 'APMrover2.txt') + results.addfile('APMrover2 code size', 'APMrover2.sizes.txt') + results.addfile('APMrover2 stack sizes', 'APMrover2.framesizes.txt') + results.addfile('APMrover2 defaults', 'APMrover2.defaults.txt') write_webresults(results) diff --git a/Tools/autotest/rover1.txt b/Tools/autotest/rover1.txt new file mode 100644 index 0000000000..bee0e3d31f --- /dev/null +++ b/Tools/autotest/rover1.txt @@ -0,0 +1,6 @@ +QGC WPL 110 +0 1 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 584.000000 1 +1 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362049 149.164810 97.790001 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363449 149.164978 97.809998 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363430 149.165359 98.580002 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361992 149.165176 98.720001 1