diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py
index 35857550b2..4ea4b04095 100644
--- a/Tools/autotest/apmrover2.py
+++ b/Tools/autotest/apmrover2.py
@@ -4,60 +4,49 @@
 from __future__ import print_function
 import os
 import shutil
-
 import pexpect
-from pymavlink import mavutil
 
 from common import *
 from pysim import util
 from pysim import vehicleinfo
+from pymavlink import mavutil, mavwp
 
 # get location of scripts
 testdir = os.path.dirname(os.path.realpath(__file__))
 
+#################################################
+# CONFIG
+#################################################
 
-# HOME=mavutil.location(-35.362938,149.165085,584,270)
+# HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
 HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246)
 homeloc = None
+num_wp = 0
+speedup_default = 10
 
 
-def arm_rover(mavproxy, mav):
-    wait_ready_to_arm(mav);
-
-    mavproxy.send('arm throttle\n')
-    mavproxy.expect('ARMED')
-
-    progress("ROVER ARMED")
-    return True
-
-
-def disarm_rover(mavproxy, mav):
-    mavproxy.send('disarm\n')
-    mavproxy.expect('DISARMED')
-
-    progress("ROVER DISARMED")
-    return True
-
-
+##########################################################
+#   TESTS DRIVE
+##########################################################
 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')
+    set_rc(mavproxy, mav, 3, 2000)
 
     progress("Driving left circuit")
     # do 4 turns
     for i in range(0, 4):
         # hard left
         progress("Starting turn %u" % i)
-        mavproxy.send('rc 1 1000\n')
+        set_rc(mavproxy, mav, 1, 1000)
         if not wait_heading(mav, 270 - (90*i), accuracy=10):
             return False
-        mavproxy.send('rc 1 1500\n')
+        set_rc(mavproxy, mav, 1, 1500)
         progress("Starting leg %u" % i)
         if not wait_distance(mav, 50, accuracy=7):
             return False
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
     progress("Circuit complete")
     return True
 
@@ -72,13 +61,9 @@ def drive_RTL(mavproxy, mav):
     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')
-
-
+#################################################
+# AUTOTEST ALL
+#################################################
 def drive_mission(mavproxy, mav, filename):
     """Drive a mission from a file."""
     global homeloc
@@ -88,7 +73,7 @@ def drive_mission(mavproxy, mav, filename):
     mavproxy.send('wp list\n')
     mavproxy.expect('Requesting [0-9]+ waypoints')
     mavproxy.send('switch 4\n')  # auto mode
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
     wait_mode(mav, 'AUTO')
     if not wait_waypoint(mav, 1, 4, max_dist=5):
         return False
@@ -96,6 +81,7 @@ def drive_mission(mavproxy, mav, filename):
     progress("Mission OK")
     return True
 
+
 def do_get_banner(mavproxy, mav):
     mavproxy.send("long DO_SEND_BANNER 1\n")
     start = time.time()
@@ -111,28 +97,6 @@ def do_get_banner(mavproxy, mav):
 
     return False
 
-def do_get_autopilot_capabilities(mavproxy, mav):
-    mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n")
-    m = mav.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=10)
-    if m is None:
-        progress("AUTOPILOT_VERSION not received")
-        return False
-    progress("AUTOPILOT_VERSION received")
-    return True;
-
-def do_set_mode_via_command_long(mavproxy, mav):
-    base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
-    custom_mode = 4 # hold
-    start = time.time()
-    while time.time() - start < 5:
-        mavproxy.send("long DO_SET_MODE %u %u\n" % (base_mode,custom_mode))
-        m = mav.recv_match(type='HEARTBEAT', blocking=True, timeout=10)
-        if m is None:
-            return False
-        if m.custom_mode == custom_mode:
-            return True
-        time.sleep(0.1)
-    return False
 
 def drive_brake_get_stopping_distance(mavproxy, mav, speed):
     # measure our stopping distance:
@@ -146,7 +110,7 @@ def drive_brake_get_stopping_distance(mavproxy, mav, speed):
     set_parameter(mavproxy, 'ATC_ACCEL_MAX', 15)
     mavproxy.send("mode STEERING\n")
     wait_mode(mav, 'STEERING')
-    mavproxy.send('rc 3 2000\n')
+    set_rc(mavproxy, mav, 3, 2000)
     wait_groundspeed(mav, 15, 100)
     initial = mav.location()
     initial_time = time.time()
@@ -155,8 +119,8 @@ def drive_brake_get_stopping_distance(mavproxy, mav, speed):
         start = mav.location()
         if start != initial:
             break
-    mavproxy.send('rc 3 1500\n')
-    wait_groundspeed(mav, 0, 0.2) # why do we not stop?!
+    set_rc(mavproxy, mav, 3, 1500)
+    wait_groundspeed(mav, 0, 0.2)  # why do we not stop?!
     initial = mav.location()
     initial_time = time.time()
     while time.time() - initial_time < 2:
@@ -171,6 +135,7 @@ def drive_brake_get_stopping_distance(mavproxy, mav, speed):
 
     return delta
 
+
 def drive_brake(mavproxy, mav):
     old_using_brake = get_parameter(mavproxy, 'ATC_BRAKE')
     old_cruise_speed = get_parameter(mavproxy, 'CRUISE_SPEED')
@@ -188,7 +153,7 @@ def drive_brake(mavproxy, mav):
     set_parameter(mavproxy, 'CRUISE_SPEED', old_cruise_speed)
 
     delta = distance_without_brakes - distance_with_brakes
-    if delta < distance_without_brakes*0.05: # 5% isn't asking for much
+    if delta < distance_without_brakes * 0.05:  # 5% isn't asking for much
         progress("Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta))
         return False
     else:
@@ -196,8 +161,10 @@ def drive_brake(mavproxy, mav):
 
     return True
 
+
 vinfo = vehicleinfo.VehicleInfo()
 
+
 def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10):
     """Drive APMrover2 in SITL.
 
@@ -217,7 +184,7 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa
 
     home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
     sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
-    mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
+    mavproxy = util.start_MAVProxy_SITL('APMrover2')
 
     progress("WAITING FOR PARAMETERS")
     mavproxy.expect('Received [0-9]+ parameters')
@@ -276,21 +243,32 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa
         progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
         mav.wait_heartbeat()
         progress("Setting up RC parameters")
-        setup_rc(mavproxy)
+        set_rc_default(mavproxy)
+        set_rc(mavproxy, mav, 8, 1800)
         progress("Waiting for GPS fix")
         mav.wait_gps_fix()
         homeloc = mav.location()
         progress("Home location: %s" % homeloc)
-        if not arm_rover(mavproxy, mav):
+        mavproxy.send('switch 6\n')  # Manual mode
+        wait_mode(mav, 'MANUAL')
+        progress("Waiting reading for arm")
+        wait_ready_to_arm(mav)
+        if not arm_vehicle(mavproxy, mav):
             progress("Failed to ARM")
             failed = True
+        progress("#")
+        progress("########## Drive a square and save WPs with CH7 switch  ##########")
+        progress("#")
+        # Drive a square in learning mode
         if not drive_mission(mavproxy, mav, os.path.join(testdir, "rover1.txt")):
             progress("Failed mission")
             failed = True
+
         if not drive_brake(mavproxy, mav):
             progress("Failed brake")
             failed = True
-        if not disarm_rover(mavproxy, mav):
+
+        if not disarm_vehicle(mavproxy, mav):
             progress("Failed to DISARM")
             failed = True
 
diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 4db06a4498..74982c7683 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -19,51 +19,32 @@ vinfo = vehicleinfo.VehicleInfo()
 # get location of scripts
 testdir = os.path.dirname(os.path.realpath(__file__))
 
+#################################################
+# CONFIG
+#################################################
 HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
 AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
 
 homeloc = None
 num_wp = 0
 
-
-def hover(mavproxy, mav, hover_throttle=1500):
-    mavproxy.send('rc 3 %u\n' % hover_throttle)
-    return True
-
-
-def arm_motors(mavproxy, mav):
-    """Arm motors."""
-    progress("Arming motors")
-    mavproxy.send('switch 6\n')  # stabilize mode
-    wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1000\n')
-    mavproxy.send('rc 4 2000\n')
-    mavproxy.expect('APM: Arming motors')
-    mavproxy.send('rc 4 1500\n')
-    mav.motors_armed_wait()
-    progress("MOTORS ARMED OK")
-    return True
-
-
-def disarm_motors(mavproxy, mav):
-    """Disarm motors."""
-    progress("Disarming motors")
-    mavproxy.send('switch 6\n')  # stabilize mode
-    wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1000\n')
-    mavproxy.send('rc 4 1000\n')
-    mavproxy.expect('APM: Disarming motors')
-    mavproxy.send('rc 4 1500\n')
-    mav.motors_disarmed_wait()
-    progress("MOTORS DISARMED OK")
-    return True
+# Flight mode switch positions are set-up in arducopter.param to be
+#   switch 1 = Circle
+#   switch 2 = Land
+#   switch 3 = RTL
+#   switch 4 = Auto
+#   switch 5 = Loiter
+#   switch 6 = Stabilize
 
 
+#################################################
+# UTILITIES
+#################################################
 def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
     """Takeoff get to 30m altitude."""
     mavproxy.send('switch 6\n')  # stabilize mode
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 %u\n' % takeoff_throttle)
+    set_rc(mavproxy, mav, 3, takeoff_throttle)
     m = mav.recv_match(type='VFR_HUD', blocking=True)
     if (m.alt < alt_min):
         wait_altitude(mav, alt_min, (alt_min + 5))
@@ -72,6 +53,22 @@ def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
     return True
 
 
+def land(mavproxy, mav, timeout=60):
+    """Land the quad."""
+    progress("STARTING LANDING")
+    mavproxy.send('switch 2\n')  # land mode
+    wait_mode(mav, 'LAND')
+    progress("Entered Landing Mode")
+    ret = wait_altitude(mav, -5, 1)
+    progress("LANDING: ok= %s" % ret)
+    return ret
+
+
+def hover(mavproxy, mav, hover_throttle=1500):
+    set_rc(mavproxy, mav, 3, hover_throttle)
+    return True
+
+
 # loiter - fly south west, then hold loiter within 5m position and altitude
 def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
     """Hold loiter position."""
@@ -80,16 +77,16 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
 
     # first aim south east
     progress("turn south east")
-    mavproxy.send('rc 4 1580\n')
+    set_rc(mavproxy, mav, 4, 1580)
     if not wait_heading(mav, 170):
         return False
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
 
     # fly south east 50m
-    mavproxy.send('rc 2 1100\n')
+    set_rc(mavproxy, mav, 2, 1100)
     if not wait_distance(mav, 50):
         return False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # wait for copter to slow moving
     if not wait_groundspeed(mav, 0, 2):
@@ -126,16 +123,19 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108
     m = mav.recv_match(type='VFR_HUD', blocking=True)
     if(m.alt < alt_min):
         progress("Rise to alt:%u from %u" % (alt_min, m.alt))
-        mavproxy.send('rc 3 %u\n' % climb_throttle)
+        set_rc(mavproxy, mav, 3, climb_throttle)
         wait_altitude(mav, alt_min, (alt_min + 5))
     else:
         progress("Lower to alt:%u from %u" % (alt_min, m.alt))
-        mavproxy.send('rc 3 %u\n' % descend_throttle)
+        set_rc(mavproxy, mav, 3, descend_throttle)
         wait_altitude(mav, (alt_min - 5), alt_min)
     hover(mavproxy, mav)
     return True
 
 
+#################################################
+#   TESTS FLY
+#################################################
 # fly a square in stabilize mode
 def fly_square(mavproxy, mav, side=50, timeout=300):
     """Fly a square, flying N then E ."""
@@ -143,10 +143,10 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
     success = True
 
     # ensure all sticks in the middle
-    mavproxy.send('rc 1 1500\n')
-    mavproxy.send('rc 2 1500\n')
-    mavproxy.send('rc 3 1500\n')
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
+    set_rc(mavproxy, mav, 2, 1500)
+    set_rc(mavproxy, mav, 3, 1500)
+    set_rc(mavproxy, mav, 4, 1500)
 
     # switch to loiter mode temporarily to stop us from rising
     mavproxy.send('switch 5\n')
@@ -154,11 +154,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
 
     # first aim north
     progress("turn right towards north")
-    mavproxy.send('rc 4 1580\n')
+    set_rc(mavproxy, mav, 4, 1580)
     if not wait_heading(mav, 10):
         progress("Failed to reach heading")
         success = False
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
     mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True)
 
     # save bottom left corner of box as waypoint
@@ -166,17 +166,17 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
     save_wp(mavproxy, mav)
 
     # switch back to stabilize mode
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
 
     # pitch forward to fly north
     progress("Going north %u meters" % side)
-    mavproxy.send('rc 2 1300\n')
+    set_rc(mavproxy, mav, 2, 1300)
     if not wait_distance(mav, side):
         progress("Failed to reach distance of %u" % side)
         success = False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # save top left corner of square as waypoint
     progress("Save WP 3")
@@ -184,11 +184,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
 
     # roll right to fly east
     progress("Going east %u meters" % side)
-    mavproxy.send('rc 1 1700\n')
+    set_rc(mavproxy, mav, 1, 1700)
     if not wait_distance(mav, side):
         progress("Failed to reach distance of %u" % side)
         success = False
-    mavproxy.send('rc 1 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
 
     # save top right corner of square as waypoint
     progress("Save WP 4")
@@ -196,11 +196,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
 
     # pitch back to fly south
     progress("Going south %u meters" % side)
-    mavproxy.send('rc 2 1700\n')
+    set_rc(mavproxy, mav, 2, 1700)
     if not wait_distance(mav, side):
         progress("Failed to reach distance of %u" % side)
         success = False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # save bottom right corner of square as waypoint
     progress("Save WP 5")
@@ -208,11 +208,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
 
     # roll left to fly west
     progress("Going west %u meters" % side)
-    mavproxy.send('rc 1 1300\n')
+    set_rc(mavproxy, mav, 1, 1300)
     if not wait_distance(mav, side):
         progress("Failed to reach distance of %u" % side)
         success = False
-    mavproxy.send('rc 1 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
 
     # save bottom left corner of square (should be near home) as waypoint
     progress("Save WP 6")
@@ -222,7 +222,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
     progress("Descend to 10m in Loiter")
     mavproxy.send('switch 5\n')  # loiter mode
     wait_mode(mav, 'LOITER')
-    mavproxy.send('rc 3 1300\n')
+    set_rc(mavproxy, mav, 3, 1300)
     time_left = timeout - (get_sim_time(mav) - tstart)
     progress("timeleft = %u" % time_left)
     if time_left < 20:
@@ -259,13 +259,13 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
 
     # first aim east
     progress("turn east")
-    mavproxy.send('rc 4 1580\n')
+    set_rc(mavproxy, mav, 4, 1580)
     if not wait_heading(mav, 135):
         return False
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
 
     # raise throttle slightly to avoid hitting the ground
-    mavproxy.send('rc 3 1600\n')
+    set_rc(mavproxy, mav, 3, 1600)
 
     # switch to stabilize mode
     mavproxy.send('switch 6\n')
@@ -274,14 +274,14 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
 
     # fly east 60 meters
     progress("# Going forward %u meters" % side)
-    mavproxy.send('rc 2 1350\n')
+    set_rc(mavproxy, mav, 2, 1350)
     if not wait_distance(mav, side, 5, 60):
         return False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # pull throttle low
     progress("# Enter Failsafe")
-    mavproxy.send('rc 3 900\n')
+    set_rc(mavproxy, mav, 3, 900)
 
     tstart = get_sim_time(mav)
     while get_sim_time(mav) < tstart + timeout:
@@ -292,7 +292,7 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
         # check if we've reached home
         if m.alt <= 1 and home_distance < 10:
             # reduce throttle
-            mavproxy.send('rc 3 1100\n')
+            set_rc(mavproxy, mav, 3, 1100)
             # switch back to stabilize
             mavproxy.send('switch 2\n')  # land mode
             wait_mode(mav, 'LAND')
@@ -301,13 +301,14 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
             progress("Reached failsafe home OK")
             mavproxy.send('switch 6\n')  # stabilize mode
             wait_mode(mav, 'STABILIZE')
-            if not arm_motors(mavproxy, mav):
+            set_rc(mavproxy, mav, 3, 1000)
+            if not arm_vehicle(mavproxy, mav):
                 progress("Failed to re-arm")
                 return False
             return True
     progress("Failed to land on failsafe RTL - timed out after %u seconds" % timeout)
     # reduce throttle
-    mavproxy.send('rc 3 1100\n')
+    set_rc(mavproxy, mav, 3, 1100)
     # switch back to stabilize mode
     mavproxy.send('switch 2\n')  # land mode
     wait_mode(mav, 'LAND')
@@ -356,16 +357,16 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
 
     # first south
     progress("turn south")
-    mavproxy.send('rc 4 1580\n')
+    set_rc(mavproxy, mav, 4, 1580)
     if not wait_heading(mav, 180):
         return False
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
 
     # fly west 80m
-    mavproxy.send('rc 2 1100\n')
+    set_rc(mavproxy, mav, 2, 1100)
     if not wait_distance(mav, 80):
         return False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # wait for copter to slow moving
     if not wait_groundspeed(mav, 0, 2):
@@ -419,14 +420,14 @@ def fly_fence_test(mavproxy, mav, timeout=180):
 
     # first east
     progress("turn east")
-    mavproxy.send('rc 4 1580\n')
+    set_rc(mavproxy, mav, 4, 1580)
     if not wait_heading(mav, 160):
         return False
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
 
     # fly forward (east) at least 20m
     pitching_forward = True
-    mavproxy.send('rc 2 1100\n')
+    set_rc(mavproxy, mav, 2, 1100)
     if not wait_distance(mav, 20):
         return False
 
@@ -440,12 +441,12 @@ def fly_fence_test(mavproxy, mav, timeout=180):
         # recenter pitch sticks once we reach home so we don't fly off again
         if pitching_forward and home_distance < 10:
             pitching_forward = False
-            mavproxy.send('rc 2 1500\n')
+            set_rc(mavproxy, mav, 2, 1500)
             # disable fence
             mavproxy.send('param set FENCE_ENABLE 0\n')
         if m.alt <= 1 and home_distance < 10:
             # reduce throttle
-            mavproxy.send('rc 3 1000\n')
+            set_rc(mavproxy, mav, 3, 1000)
             # switch mode to stabilize
             mavproxy.send('switch 2\n')  # land mode
             wait_mode(mav, 'LAND')
@@ -454,12 +455,14 @@ def fly_fence_test(mavproxy, mav, timeout=180):
             progress("Reached home OK")
             mavproxy.send('switch 6\n')  # stabilize mode
             wait_mode(mav, 'STABILIZE')
-            mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm
-            if not arm_motors(mavproxy, mav):
+            set_rc(mavproxy, mav, 3, 1000)
+            mavproxy.send('arm uncheck all\n')  # remove if we ever clear battery failsafe flag on disarm
+            if not arm_vehicle(mavproxy, mav):
                 progress("Failed to re-arm")
-                mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
+                mavproxy.send('arm check all\n')  # remove if we ever clear battery failsafe flag on disarm
                 return False
-            mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
+            mavproxy.send('arm check all\n')  # remove if we ever clear battery failsafe flag on disarm
+            progress("Reached home OK")
             return True
 
     # disable fence, enable avoidance
@@ -467,7 +470,7 @@ def fly_fence_test(mavproxy, mav, timeout=180):
     mavproxy.send('param set AVOID_ENABLE 1\n')
 
     # reduce throttle
-    mavproxy.send('rc 3 1000\n')
+    set_rc(mavproxy, mav, 3, 1000)
     # switch mode to stabilize
     mavproxy.send('switch 2\n')  # land mode
     wait_mode(mav, 'LAND')
@@ -477,17 +480,6 @@ def fly_fence_test(mavproxy, mav, timeout=180):
     return False
 
 
-def show_gps_and_sim_positions(mavproxy, on_off):
-    if on_off is True:
-        # turn on simulator display of gps and actual position
-        mavproxy.send('map set showgpspos 1\n')
-        mavproxy.send('map set showsimpos 1\n')
-    else:
-        # turn off simulator display of gps and actual position
-        mavproxy.send('map set showgpspos 0\n')
-        mavproxy.send('map set showsimpos 0\n')
-
-
 def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20):
     """fly_gps_glitch_loiter_test.
 
@@ -509,20 +501,20 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
 
     # turn south east
     progress("turn south east")
-    mavproxy.send('rc 4 1580\n')
+    set_rc(mavproxy, mav, 4, 1580)
     if not wait_heading(mav, 150):
         if (use_map):
             show_gps_and_sim_positions(mavproxy, False)
         return False
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
 
     # fly forward (south east) at least 60m
-    mavproxy.send('rc 2 1100\n')
+    set_rc(mavproxy, mav, 2, 1100)
     if not wait_distance(mav, 60):
         if (use_map):
             show_gps_and_sim_positions(mavproxy, False)
         return False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # wait for copter to slow down
     if not wait_groundspeed(mav, 0, 1):
@@ -598,7 +590,11 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
 
     # Fly mission #1
     progress("# Load copter_glitch_mission")
-    if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_glitch_mission.txt")):
+    # load the waypoint count
+    global homeloc
+    global num_wp
+    num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_glitch_mission.txt"))
+    if not num_wp:
         progress("load copter_glitch_mission failed")
         return False
 
@@ -606,16 +602,13 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
     if (use_map):
         show_gps_and_sim_positions(mavproxy, True)
 
-    # load the waypoint count
-    global homeloc
-    global num_wp
     progress("test: Fly a mission from 1 to %u" % num_wp)
     mavproxy.send('wp set 1\n')
 
     # switch into AUTO mode and raise throttle
     mavproxy.send('switch 4\n')  # auto mode
     wait_mode(mav, 'AUTO')
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
 
     # wait until 100m from home
     if not wait_distance(mav, 100, 5, 60):
@@ -692,41 +685,41 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
     # switch to stabilize mode
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
 
     # fly south 50m
     progress("# Flying south %u meters" % side)
-    mavproxy.send('rc 1 1300\n')
+    set_rc(mavproxy, mav, 1, 1300)
     if not wait_distance(mav, side, 5, 60):
         failed = True
-    mavproxy.send('rc 1 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
 
     # fly west 8 seconds
     progress("# Flying west for 8 seconds")
-    mavproxy.send('rc 2 1300\n')
+    set_rc(mavproxy, mav, 2, 1300)
     tstart = get_sim_time(mav)
     while get_sim_time(mav) < (tstart + 8):
         m = mav.recv_match(type='VFR_HUD', blocking=True)
         delta = (get_sim_time(mav) - tstart)
         # progress("%u" % delta)
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # fly north 25 meters
     progress("# Flying north %u meters" % (side/2.0))
-    mavproxy.send('rc 1 1700\n')
+    set_rc(mavproxy, mav, 1, 1700)
     if not wait_distance(mav, side/2, 5, 60):
         failed = True
-    mavproxy.send('rc 1 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
 
     # fly east 8 seconds
     progress("# Flying east for 8 seconds")
-    mavproxy.send('rc 2 1700\n')
+    set_rc(mavproxy, mav, 2, 1700)
     tstart = get_sim_time(mav)
     while get_sim_time(mav) < (tstart + 8):
         m = mav.recv_match(type='VFR_HUD', blocking=True)
         delta = (get_sim_time(mav) - tstart)
         # progress("%u" % delta)
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # restore to default
     mavproxy.send('param set SIMPLE 0\n')
@@ -747,10 +740,10 @@ def fly_super_simple(mavproxy, mav, timeout=45):
 
     # fly forward 20m
     progress("# Flying forward 20 meters")
-    mavproxy.send('rc 2 1300\n')
+    set_rc(mavproxy, mav, 2, 1300)
     if not wait_distance(mav, 20, 5, 60):
         failed = True
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # set SUPER SIMPLE mode for all flight modes
     mavproxy.send('param set SUPER_SIMPLE 63\n')
@@ -758,22 +751,22 @@ def fly_super_simple(mavproxy, mav, timeout=45):
     # switch to stabilize mode
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
 
     # start copter yawing slowly
-    mavproxy.send('rc 4 1550\n')
+    set_rc(mavproxy, mav, 4, 1550)
 
     # roll left for timeout seconds
     progress("# rolling left from pilot's point of view for %u seconds" % timeout)
-    mavproxy.send('rc 1 1300\n')
+    set_rc(mavproxy, mav, 1, 1300)
     tstart = get_sim_time(mav)
     while get_sim_time(mav) < (tstart + timeout):
         m = mav.recv_match(type='VFR_HUD', blocking=True)
         delta = (get_sim_time(mav) - tstart)
 
     # stop rolling and yawing
-    mavproxy.send('rc 1 1500\n')
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
+    set_rc(mavproxy, mav, 4, 1500)
 
     # restore simple mode parameters to default
     mavproxy.send('param set SUPER_SIMPLE 0\n')
@@ -792,21 +785,21 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
 
     # face west
     progress("turn west")
-    mavproxy.send('rc 4 1580\n')
+    set_rc(mavproxy, mav, 4, 1580)
     if not wait_heading(mav, 270):
         return False
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
 
     # set CIRCLE radius
     mavproxy.send('param set CIRCLE_RADIUS 3000\n')
 
     # fly forward (east) at least 100m
-    mavproxy.send('rc 2 1100\n')
+    set_rc(mavproxy, mav, 2, 1100)
     if not wait_distance(mav, 100):
         return False
 
     # return pitch stick back to middle
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # set CIRCLE mode
     mavproxy.send('switch 1\n')  # circle mode
@@ -831,20 +824,21 @@ def fly_auto_test(mavproxy, mav):
 
     # Fly mission #1
     progress("# Load copter_mission")
-    if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_mission.txt")):
-        progress("load copter_mission failed")
-        return False
-
     # load the waypoint count
     global homeloc
     global num_wp
+    num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_mission.txt"))
+    if not num_wp:
+        progress("load copter_mission failed")
+        return False
+
     progress("test: Fly a mission from 1 to %u" % num_wp)
     mavproxy.send('wp set 1\n')
 
     # switch into AUTO mode and raise throttle
     mavproxy.send('switch 4\n')  # auto mode
     wait_mode(mav, 'AUTO')
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
 
     # fly the mission
     ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
@@ -854,7 +848,7 @@ def fly_auto_test(mavproxy, mav):
         land(mavproxy, mav)
 
     # set throttle to minimum
-    mavproxy.send('rc 3 1000\n')
+    set_rc(mavproxy, mav, 3, 1000)
 
     # wait for disarm
     mav.motors_disarmed_wait()
@@ -870,13 +864,14 @@ def fly_avc_test(mavproxy, mav):
 
     # upload mission from file
     progress("# Load copter_AVC2013_mission")
-    if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_AVC2013_mission.txt")):
-        progress("load copter_AVC2013_mission failed")
-        return False
-
     # load the waypoint count
     global homeloc
     global num_wp
+    num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_AVC2013_mission.txt"))
+    if not num_wp:
+        progress("load copter_AVC2013_mission failed")
+        return False
+
     progress("Fly AVC mission from 1 to %u" % num_wp)
     mavproxy.send('wp set 1\n')
 
@@ -886,13 +881,13 @@ def fly_avc_test(mavproxy, mav):
     # switch into AUTO mode and raise throttle
     mavproxy.send('switch 4\n')  # auto mode
     wait_mode(mav, 'AUTO')
-    mavproxy.send('rc 3 1500\n')
+    set_rc(mavproxy, mav, 3, 1500)
 
     # fly the mission
     ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
 
     # set throttle to minimum
-    mavproxy.send('rc 3 1000\n')
+    set_rc(mavproxy, mav, 3, 1000)
 
     # wait for disarm
     mav.motors_disarmed_wait()
@@ -903,17 +898,6 @@ def fly_avc_test(mavproxy, mav):
     return ret
 
 
-def land(mavproxy, mav, timeout=60):
-    """Land the quad."""
-    progress("STARTING LANDING")
-    mavproxy.send('switch 2\n')  # land mode
-    wait_mode(mav, 'LAND')
-    progress("Entered Landing Mode")
-    ret = wait_altitude(mav, -5, 1)
-    progress("LANDING: ok= %s" % ret)
-    return ret
-
-
 def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None):
     """Fly a mission from a file."""
     global homeloc
@@ -933,38 +917,6 @@ def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None):
     return ret
 
 
-def load_mission_from_file(mavproxy, mav, filename):
-    """Load a mission from a file to flight controller."""
-    global num_wp
-    mavproxy.send('wp load %s\n' % filename)
-    mavproxy.expect('Flight plan received')
-    mavproxy.send('wp list\n')
-    mavproxy.expect('Requesting [0-9]+ waypoints')
-
-    # update num_wp
-    wploader = mavwp.MAVWPLoader()
-    wploader.load(filename)
-    num_wp = wploader.count()
-    return True
-
-
-def save_mission_to_file(mavproxy, mav, filename):
-    global num_wp
-    mavproxy.send('wp save %s\n' % filename)
-    mavproxy.expect('Saved ([0-9]+) waypoints')
-    num_wp = int(mavproxy.match.group(1))
-    progress("num_wp: %d" % num_wp)
-    return True
-
-
-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(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10):
     """Fly ArduCopter in SITL.
 
@@ -978,7 +930,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
 
     home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
     sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
-    mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
+    mavproxy = util.start_MAVProxy_SITL('ArduCopter')
     mavproxy.expect('Received [0-9]+ parameters')
 
     # setup test parameters
@@ -1041,15 +993,23 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
     failed_test_msg = "None"
 
     try:
+        progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
         mav.wait_heartbeat()
-        setup_rc(mavproxy)
+        progress("Setting up RC parameters")
+        set_rc_default(mavproxy)
+        set_rc(mavproxy, mav, 3, 1000)
         homeloc = mav.location()
 
+        progress("Home location: %s" % homeloc)
+        mavproxy.send('switch 6\n')  # stabilize mode
+        mav.wait_heartbeat()
+        wait_mode(mav, 'STABILIZE')
+        progress("Waiting reading for arm")
         wait_ready_to_arm(mav)
 
         # Arm
         progress("# Arm motors")
-        if not arm_motors(mavproxy, mav):
+        if not arm_vehicle(mavproxy, mav):
             failed_test_msg = "arm_motors failed"
             progress(failed_test_msg)
             failed = True
@@ -1071,7 +1031,9 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
 
         # save the stored mission to file
         progress("# Save out the CH7 mission to file")
-        if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
+        global num_wp
+        num_wp = save_mission_to_file(mavproxy, os.path.join(testdir, "ch7_mission.txt"))
+        if not num_wp:
             failed_test_msg = "save_mission_to_file failed"
             progress(failed_test_msg)
             failed = True
@@ -1298,8 +1260,8 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
             progress(failed_test_msg)
             failed = True
 
-    except pexpect.TIMEOUT as failed_test_msg:
-        failed_test_msg = "Timeout"
+    except pexpect.TIMEOUT as e:
+        progress("Failed with timeout")
         failed = True
 
     mav.close()
@@ -1331,7 +1293,7 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
 
     home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
     sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
-    mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
+    mavproxy = util.start_MAVProxy_SITL('ArduCopter')
     mavproxy.expect('Received [0-9]+ parameters')
 
     # setup test parameters
@@ -1397,23 +1359,26 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
 
     try:
         mav.wait_heartbeat()
-        setup_rc(mavproxy)
+        set_rc_default(mavproxy)
+        set_rc(mavproxy, mav, 3, 1000)
         homeloc = mav.location()
 
         progress("Lowering rotor speed")
-        mavproxy.send('rc 8 1000\n')
+        set_rc(mavproxy, mav, 8, 1000)
 
+        mavproxy.send('switch 6\n')  # stabilize mode
+        wait_mode(mav, 'STABILIZE')
         wait_ready_to_arm(mav)
 
         # Arm
         progress("# Arm motors")
-        if not arm_motors(mavproxy, mav):
+        if not arm_vehicle(mavproxy, mav):
             failed_test_msg = "arm_motors failed"
             progress(failed_test_msg)
             failed = True
 
         progress("Raising rotor speed")
-        mavproxy.send('rc 8 2000\n')
+        set_rc(mavproxy, mav, 8, 2000)
 
         progress("# Fly AVC mission")
         if not fly_avc_test(mavproxy, mav):
@@ -1424,7 +1389,7 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
             progress("Flew AVC mission OK")
 
         progress("Lowering rotor speed")
-        mavproxy.send('rc 8 1000\n')
+        set_rc(mavproxy, mav, 8, 1000)
 
         # mission includes disarm at end so should be ok to download logs now
         if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")):
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index c7eeddc749..28bfca026e 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -21,42 +21,41 @@ WIND = "0,180,0.2"  # speed,direction,variance
 
 homeloc = None
 
+
 def takeoff(mavproxy, mav):
     """Takeoff get to 30m altitude."""
 
     wait_ready_to_arm(mav)
-
-    mavproxy.send('arm throttle\n')
-    mavproxy.expect('ARMED')
+    arm_vehicle(mavproxy, mav)
 
     mavproxy.send('switch 4\n')
     wait_mode(mav, 'FBWA')
 
     # some rudder to counteract the prop torque
-    mavproxy.send('rc 4 1700\n')
+    set_rc(mavproxy, mav, 4, 1700)
 
     # some up elevator to keep the tail down
-    mavproxy.send('rc 2 1200\n')
+    set_rc(mavproxy, mav, 2, 1200)
 
     # get it moving a bit first
-    mavproxy.send('rc 3 1300\n')
+    set_rc(mavproxy, mav, 3, 1300)
     mav.recv_match(condition='VFR_HUD.groundspeed>6', blocking=True)
 
     # a bit faster again, straighten rudder
-    mavproxy.send('rc 3 1600\n')
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 3, 1600)
+    set_rc(mavproxy, mav, 4, 1500)
     mav.recv_match(condition='VFR_HUD.groundspeed>12', blocking=True)
 
     # hit the gas harder now, and give it some more elevator
-    mavproxy.send('rc 2 1100\n')
-    mavproxy.send('rc 3 2000\n')
+    set_rc(mavproxy, mav, 2, 1100)
+    set_rc(mavproxy, mav, 3, 2000)
 
     # gain a bit of altitude
     if not wait_altitude(mav, homeloc.alt+150, homeloc.alt+180, timeout=30):
         return False
 
     # level off
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     progress("TAKEOFF COMPLETE")
     return True
@@ -66,7 +65,7 @@ def fly_left_circuit(mavproxy, mav):
     """Fly a left circuit, 200m on a side."""
     mavproxy.send('switch 4\n')
     wait_mode(mav, 'FBWA')
-    mavproxy.send('rc 3 2000\n')
+    set_rc(mavproxy, mav, 3, 2000)
     if not wait_level_flight(mavproxy, mav):
         return False
 
@@ -75,10 +74,10 @@ def fly_left_circuit(mavproxy, mav):
     for i in range(0, 4):
         # hard left
         progress("Starting turn %u" % i)
-        mavproxy.send('rc 1 1000\n')
+        set_rc(mavproxy, mav, 1, 1000)
         if not wait_heading(mav, 270 - (90*i), accuracy=10):
             return False
-        mavproxy.send('rc 1 1500\n')
+        set_rc(mavproxy, mav, 1, 1500)
         progress("Starting leg %u" % i)
         if not wait_distance(mav, 100, accuracy=20):
             return False
@@ -169,9 +168,9 @@ def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
     """Wait for level flight."""
     tstart = get_sim_time(mav)
     progress("Waiting for level flight")
-    mavproxy.send('rc 1 1500\n')
-    mavproxy.send('rc 2 1500\n')
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
+    set_rc(mavproxy, mav, 2, 1500)
+    set_rc(mavproxy, mav, 4, 1500)
     while get_sim_time(mav) < tstart + timeout:
         m = mav.recv_match(type='ATTITUDE', blocking=True)
         roll = math.degrees(m.roll)
@@ -190,12 +189,12 @@ def change_altitude(mavproxy, mav, altitude, accuracy=30):
     wait_mode(mav, 'FBWA')
     alt_error = mav.messages['VFR_HUD'].alt - altitude
     if alt_error > 0:
-        mavproxy.send('rc 2 2000\n')
+        set_rc(mavproxy, mav, 2, 2000)
     else:
-        mavproxy.send('rc 2 1000\n')
+        set_rc(mavproxy, mav, 2, 1000)
     if not wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2):
         return False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
     progress("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt)
     return wait_level_flight(mavproxy, mav)
 
@@ -203,7 +202,7 @@ def change_altitude(mavproxy, mav, altitude, accuracy=30):
 def axial_left_roll(mavproxy, mav, count=1):
     """Fly a left axial roll."""
     # full throttle!
-    mavproxy.send('rc 3 2000\n')
+    set_rc(mavproxy, mav, 3, 2000)
     if not change_altitude(mavproxy, mav, homeloc.alt+300):
         return False
 
@@ -213,30 +212,30 @@ def axial_left_roll(mavproxy, mav, count=1):
 
     while count > 0:
         progress("Starting roll")
-        mavproxy.send('rc 1 1000\n')
+        set_rc(mavproxy, mav, 1, 1000)
         if not wait_roll(mav, -150, accuracy=90):
-            mavproxy.send('rc 1 1500\n')
+            set_rc(mavproxy, mav, 1, 1500)
             return False
         if not wait_roll(mav, 150, accuracy=90):
-            mavproxy.send('rc 1 1500\n')
+            set_rc(mavproxy, mav, 1, 1500)
             return False
         if not wait_roll(mav, 0, accuracy=90):
-            mavproxy.send('rc 1 1500\n')
+            set_rc(mavproxy, mav, 1, 1500)
             return False
         count -= 1
 
     # back to FBWA
-    mavproxy.send('rc 1 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
     mavproxy.send('switch 4\n')
     wait_mode(mav, 'FBWA')
-    mavproxy.send('rc 3 1700\n')
+    set_rc(mavproxy, mav, 3, 1700)
     return wait_level_flight(mavproxy, mav)
 
 
 def inside_loop(mavproxy, mav, count=1):
     """Fly a inside loop."""
     # full throttle!
-    mavproxy.send('rc 3 2000\n')
+    set_rc(mavproxy, mav, 3, 2000)
     if not change_altitude(mavproxy, mav, homeloc.alt+300):
         return False
 
@@ -246,7 +245,7 @@ def inside_loop(mavproxy, mav, count=1):
 
     while count > 0:
         progress("Starting loop")
-        mavproxy.send('rc 2 1000\n')
+        set_rc(mavproxy, mav, 2, 1000)
         if not wait_pitch(mav, -60, accuracy=20):
             return False
         if not wait_pitch(mav, 0, accuracy=20):
@@ -254,21 +253,21 @@ def inside_loop(mavproxy, mav, count=1):
         count -= 1
 
     # back to FBWA
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
     mavproxy.send('switch 4\n')
     wait_mode(mav, 'FBWA')
-    mavproxy.send('rc 3 1700\n')
+    set_rc(mavproxy, mav, 3, 1700)
     return wait_level_flight(mavproxy, mav)
 
 
 def test_stabilize(mavproxy, mav, count=1):
     """Fly stabilize mode."""
     # full throttle!
-    mavproxy.send('rc 3 2000\n')
-    mavproxy.send('rc 2 1300\n')
+    set_rc(mavproxy, mav, 3, 2000)
+    set_rc(mavproxy, mav, 2, 1300)
     if not change_altitude(mavproxy, mav, homeloc.alt+300):
         return False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     mavproxy.send("mode STABILIZE\n")
     wait_mode(mav, 'STABILIZE')
@@ -276,7 +275,7 @@ def test_stabilize(mavproxy, mav, count=1):
     count = 1
     while count > 0:
         progress("Starting roll")
-        mavproxy.send('rc 1 2000\n')
+        set_rc(mavproxy, mav, 1, 2000)
         if not wait_roll(mav, -150, accuracy=90):
             return False
         if not wait_roll(mav, 150, accuracy=90):
@@ -285,25 +284,25 @@ def test_stabilize(mavproxy, mav, count=1):
             return False
         count -= 1
 
-    mavproxy.send('rc 1 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
     if not wait_roll(mav, 0, accuracy=5):
         return False
 
     # back to FBWA
     mavproxy.send('mode FBWA\n')
     wait_mode(mav, 'FBWA')
-    mavproxy.send('rc 3 1700\n')
+    set_rc(mavproxy, mav, 3, 1700)
     return wait_level_flight(mavproxy, mav)
 
 
 def test_acro(mavproxy, mav, count=1):
     """Fly ACRO mode."""
     # full throttle!
-    mavproxy.send('rc 3 2000\n')
-    mavproxy.send('rc 2 1300\n')
+    set_rc(mavproxy, mav, 3, 2000)
+    set_rc(mavproxy, mav, 2, 1300)
     if not change_altitude(mavproxy, mav, homeloc.alt+300):
         return False
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     mavproxy.send("mode ACRO\n")
     wait_mode(mav, 'ACRO')
@@ -311,7 +310,7 @@ def test_acro(mavproxy, mav, count=1):
     count = 1
     while count > 0:
         progress("Starting roll")
-        mavproxy.send('rc 1 1000\n')
+        set_rc(mavproxy, mav, 1, 1000)
         if not wait_roll(mav, -150, accuracy=90):
             return False
         if not wait_roll(mav, 150, accuracy=90):
@@ -319,7 +318,7 @@ def test_acro(mavproxy, mav, count=1):
         if not wait_roll(mav, 0, accuracy=90):
             return False
         count -= 1
-    mavproxy.send('rc 1 1500\n')
+    set_rc(mavproxy, mav, 1, 1500)
 
     # back to FBWA
     mavproxy.send('mode FBWA\n')
@@ -333,19 +332,19 @@ def test_acro(mavproxy, mav, count=1):
     count = 2
     while count > 0:
         progress("Starting loop")
-        mavproxy.send('rc 2 1000\n')
+        set_rc(mavproxy, mav, 2, 1000)
         if not wait_pitch(mav, -60, accuracy=20):
             return False
         if not wait_pitch(mav, 0, accuracy=20):
             return False
         count -= 1
 
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
 
     # back to FBWA
     mavproxy.send('mode FBWA\n')
     wait_mode(mav, 'FBWA')
-    mavproxy.send('rc 3 1700\n')
+    set_rc(mavproxy, mav, 3, 1700)
     return wait_level_flight(mavproxy, mav)
 
 
@@ -353,13 +352,13 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
     """Fly FBWB or CRUISE mode."""
     mavproxy.send("mode %s\n" % mode)
     wait_mode(mav, mode)
-    mavproxy.send('rc 3 1700\n')
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 3, 1700)
+    set_rc(mavproxy, mav, 2, 1500)
 
     # lock in the altitude by asking for an altitude change then releasing
-    mavproxy.send('rc 2 1000\n')
+    set_rc(mavproxy, mav, 2, 1000)
     wait_distance(mav, 50, accuracy=20)
-    mavproxy.send('rc 2 1500\n')
+    set_rc(mavproxy, mav, 2, 1500)
     wait_distance(mav, 50, accuracy=20)
 
     m = mav.recv_match(type='VFR_HUD', blocking=True)
@@ -371,11 +370,11 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
     for i in range(0, 4):
         # hard left
         progress("Starting turn %u" % i)
-        mavproxy.send('rc 1 1800\n')
+        set_rc(mavproxy, mav, 1, 1800)
         if not wait_heading(mav, 0 + (90*i), accuracy=20, timeout=60):
-            mavproxy.send('rc 1 1500\n')
+            set_rc(mavproxy, mav, 1, 1500)
             return False
-        mavproxy.send('rc 1 1500\n')
+        set_rc(mavproxy, mav, 1, 1500)
         progress("Starting leg %u" % i)
         if not wait_distance(mav, 100, accuracy=20):
             return False
@@ -386,11 +385,11 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
     for i in range(0, 4):
         # hard left
         progress("Starting turn %u" % i)
-        mavproxy.send('rc 4 1900\n')
+        set_rc(mavproxy, mav, 4, 1900)
         if not wait_heading(mav, 360 - (90*i), accuracy=20, timeout=60):
-            mavproxy.send('rc 4 1500\n')
+            set_rc(mavproxy, mav, 4, 1500)
             return False
-        mavproxy.send('rc 4 1500\n')
+        set_rc(mavproxy, mav, 4, 1500)
         progress("Starting leg %u" % i)
         if not wait_distance(mav, 100, accuracy=20):
             return False
@@ -411,14 +410,6 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
     return wait_level_flight(mavproxy, mav)
 
 
-def setup_rc(mavproxy):
-    """Setup RC override control."""
-    for chan in [1, 2, 4, 5, 6, 7]:
-        mavproxy.send('rc %u 1500\n' % chan)
-    mavproxy.send('rc 3 1000\n')
-    mavproxy.send('rc 8 1800\n')
-
-
 def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
     """Fly a mission from a file."""
     global homeloc
@@ -494,7 +485,9 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
         progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
         mav.wait_heartbeat()
         progress("Setting up RC parameters")
-        setup_rc(mavproxy)
+        set_rc_default(mavproxy)
+        set_rc(mavproxy, mav, 3, 1000)
+        set_rc(mavproxy, mav, 8, 1800)
         progress("Waiting for GPS fix")
         mav.recv_match(condition='VFR_HUD.alt>10', blocking=True)
         mav.wait_gps_fix()
@@ -546,7 +539,7 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
             progress("Failed CIRCLE")
             failed = True
             fail_list.append("LOITER")
-        if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10,
+        if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy=10,
                            target_altitude=homeloc.alt+100):
             progress("Failed mission")
             failed = True
diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index babec83c82..93d6c2a7e0 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -19,55 +19,43 @@ HOME = mavutil.location(33.810313, -118.393867, 0, 185)
 homeloc = None
 
 
-def arm_sub(mavproxy, mav):
-    for i in range(8):
-        mavproxy.send('rc %d 1500\n' % (i+1))
-
-    mavproxy.send('arm throttle\n')
-    mavproxy.expect('ARMED')
-
-    progress("SUB ARMED")
-    return True
-
 def dive_manual(mavproxy, mav):
-    mavproxy.send('rc 3 1600\n')
-    mavproxy.send('rc 5 1600\n')
-    mavproxy.send('rc 6 1550\n')
+    set_rc(mavproxy, mav, 3, 1600)
+    set_rc(mavproxy, mav, 5, 1600)
+    set_rc(mavproxy, mav, 6, 1550)
 
     if not wait_distance(mav, 50, accuracy=7, timeout=200):
         return False
     
-    mavproxy.send('rc 4 1550\n')
+    set_rc(mavproxy, mav, 4, 1550)
     
     if not wait_heading(mav, 0):
         return False
     
-    mavproxy.send('rc 4 1500\n')
+    set_rc(mavproxy, mav, 4, 1500)
     
     if not wait_distance(mav, 50, accuracy=7, timeout=100):
         return False
     
-    mavproxy.send('rc 4 1550\n')
+    set_rc(mavproxy, mav, 4, 1550)
     
     if not wait_heading(mav, 0):
         return False
     
-    mavproxy.send('rc 4 1500\n')
-    mavproxy.send('rc 5 1500\n')
-    mavproxy.send('rc 6 1100\n')
+    set_rc(mavproxy, mav, 4, 1500)
+    set_rc(mavproxy, mav, 5, 1500)
+    set_rc(mavproxy, mav, 6, 1100)
     
     if not wait_distance(mav, 75, accuracy=7, timeout=100):
         return False
-    
-    mavproxy.send('rc all 1500\n')
-    
-    mavproxy.send('disarm\n');
 
-    # wait for disarm
-    mav.motors_disarmed_wait()
+    set_rc_default(mavproxy)
+
+    disarm_vehicle(mavproxy, mav)
     progress("Manual dive OK")
     return True
 
+
 def dive_mission(mavproxy, mav, filename):
     
     progress("Executing mission %s" % filename)
@@ -75,8 +63,9 @@ def dive_mission(mavproxy, mav, filename):
     mavproxy.expect('Flight plan received')
     mavproxy.send('wp list\n')
     mavproxy.expect('Saved [0-9]+ waypoints')
+    set_rc_default(mavproxy)
     
-    if not arm_sub(mavproxy, mav):
+    if not arm_vehicle(mavproxy, mav):
         progress("Failed to ARM")
         return False
     
@@ -85,15 +74,13 @@ def dive_mission(mavproxy, mav, filename):
     
     if not wait_waypoint(mav, 1, 5, max_dist=5):
         return False
-    
-    mavproxy.send('disarm\n');
-    
-    # wait for disarm
-    mav.motors_disarmed_wait()
+
+    disarm_vehicle(mavproxy, mav)
 
     progress("Mission OK")
     return True
 
+
 def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, gdbserver=False, speedup=10):
     """Dive ArduSub in SITL.
 
@@ -108,7 +95,7 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False
 
     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=speedup)
-    mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
+    mavproxy = util.start_MAVProxy_SITL('ArduSub')
     mavproxy.expect('Received [0-9]+ parameters')
 
     # setup test parameters
@@ -169,7 +156,8 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False
         
         homeloc = mav.location()
         progress("Home location: %s" % homeloc)
-        if not arm_sub(mavproxy, mav):
+        set_rc_default(mavproxy)
+        if not arm_vehicle(mavproxy, mav):
             progress("Failed to ARM")
             failed = True
         if not dive_manual(mavproxy, mav):
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index f281b7105e..39e8c824cb 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -2,7 +2,7 @@ from __future__ import print_function
 import math
 import time
 
-from pymavlink import mavwp
+from pymavlink import mavwp, mavutil
 
 from pysim import util
 
@@ -10,30 +10,14 @@ from pysim import util
 # messages. This keeps the output to stdout flowing
 expect_list = []
 
+
 class AutoTestTimeoutException(Exception):
     pass
 
-def wait_ready_to_arm(mav, timeout=None):
-    # wait for EKF checks to pass
-    return wait_ekf_happy(mav, timeout=timeout)
-
-def wait_ekf_happy(mav, timeout=30):
-    """Wait for EKF to be happy"""
-
-    tstart = get_sim_time(mav)
-    required_value = 831
-    progress("Waiting for EKF value %u" % (required_value))
-    while timeout is None or get_sim_time(mav) < tstart + timeout:
-        m = mav.recv_match(type='EKF_STATUS_REPORT', blocking=True)
-        current = m.flags
-        if (tstart - get_sim_time(mav)) % 5 == 0:
-            progress("Wait EKF.flags: required:%u current:%u" % (required_value, current))
-        if current == required_value:
-            progress("EKF Flags OK")
-            return
-    progress("Failed to get EKF.flags=%u" % required_value)
-    raise AutoTestTimeoutException()
 
+#################################################
+# GENERAL UTILITIES
+#################################################
 def expect_list_clear():
     """clear the expect list."""
     global expect_list
@@ -68,13 +52,152 @@ def expect_callback(e):
         util.pexpect_drain(p)
 
 
-'''
-SIM UTILITIES
-'''
+#################################################
+# SIM UTILITIES
+#################################################
 def progress(text):
     """Display autotest progress text."""
     print("AUTOTEST: " + text)
 
+
+def get_sim_time(mav):
+    """Get SITL time."""
+    m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
+    return m.time_boot_ms * 1.0e-3
+
+
+def sim_location(mav):
+    """Return current simulator location."""
+    m = mav.recv_match(type='SIMSTATE', blocking=True)
+    return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
+
+
+def save_wp(mavproxy, mav):
+    """Trigger RC 7 to save waypoint."""
+    mavproxy.send('rc 7 1000\n')
+    mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
+    wait_seconds(mav, 1)
+    mavproxy.send('rc 7 2000\n')
+    mav.recv_match(condition='RC_CHANNELS.chan7_raw==2000', blocking=True)
+    wait_seconds(mav, 1)
+    mavproxy.send('rc 7 1000\n')
+    mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
+    wait_seconds(mav, 1)
+
+
+def log_download(mavproxy, mav, filename, timeout=360):
+    """Download latest log."""
+    mavproxy.send("log list\n")
+    mavproxy.expect("numLogs")
+    mav.wait_heartbeat()
+    mav.wait_heartbeat()
+    mavproxy.send("set shownoise 0\n")
+    mavproxy.send("log download latest %s\n" % filename)
+    mavproxy.expect("Finished downloading", timeout=timeout)
+    mav.wait_heartbeat()
+    mav.wait_heartbeat()
+    return True
+
+
+def show_gps_and_sim_positions(mavproxy, on_off):
+    """Allow to display gps and actual position on map."""
+    if on_off is True:
+        # turn on simulator display of gps and actual position
+        mavproxy.send('map set showgpspos 1\n')
+        mavproxy.send('map set showsimpos 1\n')
+    else:
+        # turn off simulator display of gps and actual position
+        mavproxy.send('map set showgpspos 0\n')
+        mavproxy.send('map set showsimpos 0\n')
+
+
+def mission_count(filename):
+    """Load a mission from a file and return number of waypoints."""
+    wploader = mavwp.MAVWPLoader()
+    wploader.load(filename)
+    num_wp = wploader.count()
+    return num_wp
+
+
+def load_mission_from_file(mavproxy, filename):
+    """Load a mission from a file to flight controller."""
+    mavproxy.send('wp load %s\n' % filename)
+    mavproxy.expect('Flight plan received')
+    mavproxy.send('wp list\n')
+    mavproxy.expect('Requesting [0-9]+ waypoints')
+
+    # update num_wp
+    wploader = mavwp.MAVWPLoader()
+    wploader.load(filename)
+    num_wp = wploader.count()
+    return num_wp
+
+
+def save_mission_to_file(mavproxy, filename):
+    """Save a mission to a file"""
+    mavproxy.send('wp save %s\n' % filename)
+    mavproxy.expect('Saved ([0-9]+) waypoints')
+    num_wp = int(mavproxy.match.group(1))
+    progress("num_wp: %d" % num_wp)
+    return num_wp
+
+
+def set_rc_default(mavproxy):
+    """Setup all simulated RC control to 1500."""
+    for chan in range(1, 16):
+        mavproxy.send('rc %u 1500\n' % chan)
+
+
+def set_rc(mavproxy, mav, chan, pwm, timeout=2):
+    """Setup a simulated RC control to a PWM value"""
+    tstart = get_sim_time(mav)
+    while get_sim_time(mav) < tstart + timeout:
+        mavproxy.send('rc %u %u\n' % (chan, pwm))
+        m = mav.recv_match(type='RC_CHANNELS', blocking=True)
+        chan_pwm = getattr(m, "chan" + str(chan) + "_raw")
+        if chan_pwm == pwm:
+            return True
+    progress("Failed to send RC commands")
+    return False
+
+
+def arm_vehicle(mavproxy, mav):
+    """Arm vehicle with mavlink arm message."""
+    mavproxy.send('arm throttle\n')
+    mav.motors_armed_wait()
+    progress("ARMED")
+    return True
+
+
+def disarm_vehicle(mavproxy, mav):
+    """Disarm vehicle with mavlink disarm message."""
+    mavproxy.send('disarm\n')
+    mav.motors_disarmed_wait()
+    progress("DISARMED")
+    return True
+
+
+def set_parameter(mavproxy, name, value):
+    for i in range(1, 10):
+        mavproxy.send("param set %s %s\n" % (name, str(value)))
+        mavproxy.send("param fetch %s\n" % (name))
+        mavproxy.expect("%s = (.*)" % (name,))
+        returned_value = mavproxy.match.group(1)
+        if float(returned_value) == float(value):
+            # yes, exactly equal.
+            break
+        progress("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value))
+
+
+def get_parameter(mavproxy, name):
+    mavproxy.send("param fetch %s\n" % (name))
+    mavproxy.expect("%s = (.*)" % (name,))
+    return float(mavproxy.match.group(1))
+
+
+#################################################
+# UTILITIES
+#################################################
 def get_distance(loc1, loc2):
     """Get ground distance between two locations."""
     dlat = loc2.lat - loc1.lat
@@ -92,33 +215,42 @@ def get_bearing(loc1, loc2):
     return bearing
 
 
+def do_get_autopilot_capabilities(mavproxy, mav):
+    mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n")
+    m = mav.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=10)
+    if m is None:
+        progress("AUTOPILOT_VERSION not received")
+        return False
+    progress("AUTOPILOT_VERSION received")
+    return True
+
+
+def do_set_mode_via_command_long(mavproxy, mav):
+    base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
+    custom_mode = 4  # hold
+    start = time.time()
+    while time.time() - start < 5:
+        mavproxy.send("long DO_SET_MODE %u %u\n" % (base_mode,custom_mode))
+        m = mav.recv_match(type='HEARTBEAT', blocking=True, timeout=10)
+        if m is None:
+            return False
+        if m.custom_mode == custom_mode:
+            return True
+        time.sleep(0.1)
+    return False
+
+
+#################################################
+# WAIT UTILITIES
+#################################################
 def wait_seconds(mav, seconds_to_wait):
+    """Wait some second in SITL time."""
     tstart = get_sim_time(mav)
     tnow = tstart
     while tstart + seconds_to_wait > tnow:
         tnow = get_sim_time(mav)
 
 
-def get_sim_time(mav):
-    m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
-    return m.time_boot_ms * 1.0e-3
-
-def set_parameter(mavproxy, name ,value):
-    for i in range(1,10):
-        mavproxy.send("param set %s %s\n" % (name, str(value)))
-        mavproxy.send("param fetch %s\n" % (name))
-        mavproxy.expect("%s = (.*)" % (name,))
-        returned_value = mavproxy.match.group(1)
-        if float(returned_value) == float(value):
-            # yes, exactly equal.
-            break
-        progress("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value))
-
-def get_parameter(mavproxy, name):
-    mavproxy.send("param fetch %s\n" % (name))
-    mavproxy.expect("%s = (.*)" % (name,))
-    return float(mavproxy.match.group(1))
-
 def wait_altitude(mav, alt_min, alt_max, timeout=30):
     """Wait for a given altitude range."""
     climb_rate = 0
@@ -280,49 +412,33 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
     return False
 
 
-def save_wp(mavproxy, mav):
-    mavproxy.send('rc 7 1000\n')
-    mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
-    wait_seconds(mav, 1)
-    mavproxy.send('rc 7 2000\n')
-    mav.recv_match(condition='RC_CHANNELS.chan7_raw==2000', blocking=True)
-    wait_seconds(mav, 1)
-    mavproxy.send('rc 7 1000\n')
-    mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
-    wait_seconds(mav, 1)
-
-
 def wait_mode(mav, mode, timeout=None):
+    """Wait for mode to change."""
     progress("Waiting for mode %s" % mode)
+    mav.wait_heartbeat()
     mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True)
     progress("Got mode %s" % mode)
     return mav.flightmode
 
 
-def mission_count(filename):
-    """Load a mission from a file and return number of waypoints."""
-    wploader = mavwp.MAVWPLoader()
-    wploader.load(filename)
-    num_wp = wploader.count()
-    return num_wp
-
+def wait_ready_to_arm(mav, timeout=None):
+    # wait for EKF checks to pass
+    return wait_ekf_happy(mav, timeout=timeout)
 
-def sim_location(mav):
-    """Return current simulator location."""
-    from pymavlink import mavutil
-    m = mav.recv_match(type='SIMSTATE', blocking=True)
-    return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
 
+def wait_ekf_happy(mav, timeout=30):
+    """Wait for EKF to be happy"""
 
-def log_download(mavproxy, mav, filename, timeout=360):
-    """Download latest log."""
-    mavproxy.send("log list\n")
-    mavproxy.expect("numLogs")
-    mav.wait_heartbeat()
-    mav.wait_heartbeat()
-    mavproxy.send("set shownoise 0\n")
-    mavproxy.send("log download latest %s\n" % filename)
-    mavproxy.expect("Finished downloading", timeout=timeout)
-    mav.wait_heartbeat()
-    mav.wait_heartbeat()
-    return True
+    tstart = get_sim_time(mav)
+    required_value = 831
+    progress("Waiting for EKF value %u" % (required_value))
+    while timeout is None or get_sim_time(mav) < tstart + timeout:
+        m = mav.recv_match(type='EKF_STATUS_REPORT', blocking=True)
+        current = m.flags
+        if (tstart - get_sim_time(mav)) % 5 == 0:
+            progress("Wait EKF.flags: required:%u current:%u" % (required_value, current))
+        if current == required_value:
+            progress("EKF Flags OK")
+            return
+    progress("Failed to get EKF.flags=%u" % required_value)
+    raise AutoTestTimeoutException()
diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py
index 4c38d7599c..0b7e2f194e 100644
--- a/Tools/autotest/quadplane.py
+++ b/Tools/autotest/quadplane.py
@@ -37,8 +37,7 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
     mavproxy.expect('DISARMED')
     # wait for blood sample here
     mavproxy.send('wp set 20\n')
-    mavproxy.send('arm throttle\n')
-    mavproxy.expect('ARMED')
+    arm_vehicle(mavproxy, mav)
     if not wait_waypoint(mav, 20, 34, max_dist=60, timeout=1200):
         return False
     mavproxy.expect('DISARMED')
@@ -110,8 +109,7 @@ def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
         # wait for EKF and GPS checks to pass
         wait_seconds(mav, 30)
 
-        mavproxy.send('arm throttle\n')
-        mavproxy.expect('ARMED')
+        arm_vehicle(mavproxy, mav)
 
         if not fly_mission(mavproxy, mav,
                            os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"),