diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
new file mode 100644
index 0000000000..9061313256
--- /dev/null
+++ b/Tools/autotest/ardusub.py
@@ -0,0 +1,180 @@
+# drive APMrover2 in SITL
+from __future__ import print_function
+import os
+import shutil
+
+import pexpect
+from pymavlink import mavutil
+
+from common import *
+from pysim import util
+
+# get location of scripts
+testdir = os.path.dirname(os.path.realpath(__file__))
+
+
+HOME = mavutil.location(33.810313, -118.393867, 0, 185)
+homeloc = None
+
+
+def wait_ready_to_arm(mavproxy):
+    # wait for EKF and GPS checks to pass
+    mavproxy.expect('IMU0 is using GPS')
+
+def arm_sub(mavproxy, mav):
+    wait_ready_to_arm(mavproxy);
+    for i in range(8):
+        mavproxy.send('rc %d 1500\n' % (i+1))
+
+    mavproxy.send('arm throttle\n')
+    mavproxy.expect('ARMED')
+
+    print("SUB ARMED")
+    return True
+
+def dive_mission(mavproxy, mav, filename):
+    """Dive a mission from a file."""
+    global homeloc
+
+    mavproxy.send('rc 3 1600\n')
+    mavproxy.send('rc 6 1600\n')
+    mavproxy.send('rc 7 1550\n')
+
+    if not wait_distance(mav, 50, accuracy=7, timeout=100):
+        return False
+    
+    mavproxy.send('rc 4 1550\n')
+    
+    if not wait_heading(mav, 0):
+        return False
+    
+    mavproxy.send('rc 4 1500\n')
+    
+    if not wait_distance(mav, 50, accuracy=7, timeout=100):
+        return False
+    
+    mavproxy.send('rc 4 1550\n')
+    
+    if not wait_heading(mav, 0):
+        return False
+    
+    mavproxy.send('rc 4 1500\n')
+    mavproxy.send('rc 6 1500\n')
+    mavproxy.send('rc 7 1100\n')
+    
+    if not wait_distance(mav, 75, accuracy=7, timeout=100):
+        return False
+    
+    mavproxy.send('rc 7 1500\n')
+    
+    wait_mode(mav, 'MANUAL')
+    print("Mission OK")
+    return True
+
+
+def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
+    """Dive ArduSub in SITL.
+
+    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
+    if use_map:
+        options += ' --map'
+
+    home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
+    sitl = util.start_SITL(binary, model='vectored', wipe=True, home=home, speedup=10)
+    mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
+    mavproxy.expect('Received [0-9]+ parameters')
+
+    # setup test parameters
+    mavproxy.send("param load %s/default_params/sub.parm\n" % testdir)
+    mavproxy.expect('Loaded [0-9]+ parameters')
+    mavproxy.send('param set FS_GCS_ENABLE 0\n')
+    mavproxy.send("param set LOG_REPLAY 1\n")
+    mavproxy.send("param set LOG_DISARMED 1\n")
+    time.sleep(3)
+
+    # reboot with new parameters
+    util.pexpect_close(mavproxy)
+    util.pexpect_close(sitl)
+
+    sitl = util.start_SITL(binary, model='vectored', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
+    mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
+    mavproxy.expect('Telemetry log: (\S+)')
+    logfile = mavproxy.match.group(1)
+    print("LOGFILE %s" % logfile)
+
+    buildlog = util.reltopdir("../buildlogs/ArduSub-test.tlog")
+    print("buildlog=%s" % buildlog)
+    if os.path.exists(buildlog):
+        os.unlink(buildlog)
+    try:
+        os.link(logfile, buildlog)
+    except Exception:
+        pass
+
+    mavproxy.expect('Received [0-9]+ parameters')
+
+    util.expect_setup_callback(mavproxy, expect_callback)
+
+    expect_list_clear()
+    expect_list_extend([sitl, mavproxy])
+
+    print("Started simulator")
+
+    # get a mavlink connection going
+    try:
+        mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
+    except Exception as 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("Waiting for GPS fix")
+        mav.wait_gps_fix()
+
+        homeloc = mav.location()
+        print("Home location: %s" % homeloc)
+        if not arm_sub(mavproxy, mav):
+            print("Failed to ARM")
+            failed = True
+        if not dive_mission(mavproxy, mav, os.path.join(testdir, "sub1.txt")):
+            print("Failed mission")
+            failed = True
+        
+        mavproxy.send('disarm\n');
+        
+        # wait for disarm
+        mav.motors_disarmed_wait()
+        
+        if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduSub-log.bin")):
+            print("Failed log download")
+            failed = True
+    except pexpect.TIMEOUT as e:
+        print("Failed with timeout")
+        failed = True
+
+    mav.close()
+    util.pexpect_close(mavproxy)
+    util.pexpect_close(sitl)
+
+    valgrind_log = util.valgrind_log_filepath(binary=binary, model='sub')
+    if os.path.exists(valgrind_log):
+        os.chmod(valgrind_log, 0o644)
+        shutil.copy(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 c5477c4f22..5a9618845b 100755
--- a/Tools/autotest/autotest.py
+++ b/Tools/autotest/autotest.py
@@ -19,6 +19,7 @@ import apmrover2
 import arducopter
 import arduplane
 import quadplane
+import ardusub
 from pysim import util
 
 os.environ['PYTHONUNBUFFERED'] = '1'
@@ -196,6 +197,10 @@ steps = [
     'fly.CopterAVC',
 
     'build.AntennaTracker',
+    
+    'build.ArduSub',
+    'defaults.ArduSub',
+    'dive.ArduSub',
 
     'convertgpx',
     ]
@@ -233,6 +238,8 @@ def binary_path(step, debug=False):
         binary_name = "arducopter-heli"
     elif step.find("QuadPlane") != -1:
         binary_name = "arduplane"
+    elif step.find("ArduSub") != -1:
+        binary_name = "ardusub"
     else:
         # cope with builds that don't have a specific binary
         return None
@@ -275,6 +282,9 @@ def run_step(step):
 
     if step == 'build.Helicopter':
         return util.build_SITL('bin/arducopter-heli', j=opts.j, debug=opts.debug)
+    
+    if step == 'build.ArduSub':
+        return util.build_SITL('bin/ardusub', j=opts.j, debug=opts.debug)
 
     binary = binary_path(step, debug=opts.debug)
 
@@ -286,6 +296,9 @@ def run_step(step):
 
     if step == 'defaults.APMrover2':
         return get_default_params('APMrover2', binary)
+    
+    if step == 'defaults.ArduSub':
+        return get_default_params('ArduSub', binary)
 
     if step == 'fly.ArduCopter':
         return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
@@ -301,6 +314,9 @@ def run_step(step):
 
     if step == 'drive.APMrover2':
         return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
+    
+    if step == 'dive.ArduSub':
+        return ardusub.dive_ArduSub(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
 
     if step == 'build.All':
         return build_all()
@@ -420,10 +436,18 @@ def write_fullresults():
     results.addfile('AntennaTracker code size', 'AntennaTracker.sizes.txt')
     results.addfile('AntennaTracker stack sizes', 'AntennaTracker.framesizes.txt')
     results.addglob("AntennaTracker ELF", 'AntennaTracker.elf')
+    results.addfile('ArduSub build log', 'ArduSub.txt')
+    results.addfile('ArduSub code size', 'ArduSub.sizes.txt')
+    results.addfile('ArduSub stack sizes', 'ArduSub.framesizes.txt')
+    results.addfile('ArduSub defaults', 'default_params/ArduSub-defaults.parm')
+    results.addglob("ArduSub log", 'ArduSub-*.BIN')
+    results.addglob("ArduSub core", 'ArduSub.core')
+    results.addglob("ArduSub ELF", 'ArduSub.elf')
     results.addglob('APM:Libraries documentation', 'docs/libraries/index.html')
     results.addglob('APM:Plane documentation', 'docs/ArduPlane/index.html')
     results.addglob('APM:Copter documentation', 'docs/ArduCopter/index.html')
     results.addglob('APM:Rover documentation', 'docs/APMrover2/index.html')
+    results.addglob('APM:Sub documentation', 'docs/ArduSub/index.html')
     results.addglobimage("Flight Track", '*.png')
 
     write_webresults(results)
diff --git a/Tools/scripts/build_all_px4.sh b/Tools/scripts/build_all_px4.sh
index 45f3d98617..ab5057a3ab 100755
--- a/Tools/scripts/build_all_px4.sh
+++ b/Tools/scripts/build_all_px4.sh
@@ -11,7 +11,7 @@ set -x
 git submodule init
 git submodule update
 
-for d in ArduPlane ArduCopter APMrover2; do
+for d in ArduPlane ArduCopter APMrover2 ArduSub; do
     pushd $d
     make px4-clean
     popd
@@ -32,4 +32,9 @@ pushd APMrover2
 make px4
 popd
 
+echo "Testing ArduSub build"
+pushd ArduSub
+make px4
+popd
+
 exit 0