autotest: added Rover testing in autotest

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

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

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)
 

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