diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 0a69efde32..92683fe8a7 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -9,7 +9,7 @@ testdir=os.path.dirname(os.path.realpath(__file__))
 sys.path.insert(0, util.reltopdir('../pymavlink'))
 import mavutil, mavwp
 
-HOME_LOCATION='-35.362938,149.165085,584,270'
+HOME=location(-35.362938,149.165085,584,270)
 
 homeloc = None
 num_wp = 0
@@ -231,6 +231,11 @@ def fly_ArduCopter(viewerip=None):
     '''
     global expect_list, homeloc
 
+    hquad_cmd = util.reltopdir('../HILTest/hil_quad.py') + ' --fgrate=200 --home=%f,%f,%u,%u' % (
+        HOME.lat, HOME.lng, HOME.alt, HOME.heading)
+    if viewerip:
+        hquad_cmd += ' --fgout=192.168.2.15:9123'
+
     sil = util.start_SIL('ArduCopter', wipe=True)
     mavproxy = util.start_MAVProxy_SIL('ArduCopter')
     mavproxy.expect('Please Run Setup')
@@ -239,7 +244,7 @@ def fly_ArduCopter(viewerip=None):
     util.pexpect_close(mavproxy)
     util.pexpect_close(sil)
     sil = util.start_SIL('ArduCopter')
-    mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
+    mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
     mavproxy.expect('Received [0-9]+ parameters')
 
     # setup test parameters
@@ -249,8 +254,11 @@ def fly_ArduCopter(viewerip=None):
     # reboot with new parameters
     util.pexpect_close(mavproxy)
     util.pexpect_close(sil)
-    sil = util.start_SIL('ArduCopter')
-    options = '--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=1'
+    
+    sil = util.start_SIL('ArduCopter', height=HOME.alt)
+    hquad = pexpect.spawn(hquad_cmd, logfile=sys.stdout, timeout=10)
+    util.pexpect_autoclose(hquad)
+    options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=1'
     if viewerip:
         options += ' --out=%s:14550' % viewerip
     mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
@@ -264,19 +272,11 @@ def fly_ArduCopter(viewerip=None):
         os.unlink(buildlog)
     os.link(logfile, buildlog)
 
-    mavproxy.expect("Ready to FLY")
     mavproxy.expect('Received [0-9]+ parameters')
+    mavproxy.expect("Ready to FLY")
 
     util.expect_setup_callback(mavproxy, expect_callback)
 
-    # start hil_quad.py
-    cmd = util.reltopdir('../HILTest/hil_quad.py') + ' --fgrate=200 --home=%s' % HOME_LOCATION
-    if viewerip:
-        cmd += ' --fgout=192.168.2.15:9123'
-    hquad = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
-    util.pexpect_autoclose(hquad)
-    hquad.expect('Starting at')
-
     expect_list.extend([hquad, sil, mavproxy])
 
     # get a mavlink connection going
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 3296f23332..c89be9282c 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -30,10 +30,11 @@ def expect_callback(e):
 
 class location(object):
     '''represent a GPS coordinate'''
-    def __init__(self, lat, lng, alt=0):
+    def __init__(self, lat, lng, alt=0, heading=0):
         self.lat = lat
         self.lng = lng
         self.alt = alt
+        self.heading = heading
 
     def __str__(self):
         return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt)
diff --git a/Tools/autotest/util.py b/Tools/autotest/util.py
index 7c1d42df2f..883348ca80 100644
--- a/Tools/autotest/util.py
+++ b/Tools/autotest/util.py
@@ -44,7 +44,7 @@ def deltree(path):
 
 def build_SIL(atype):
     '''build desktop SIL'''
-    run_cmd("make -f ../libraries/Desktop/Makefile.desktop clean hil",
+    run_cmd("make -f ../libraries/Desktop/Makefile.desktop clean all",
             dir=reltopdir(atype),
             checkfail=True)
     return True
@@ -95,7 +95,7 @@ def pexpect_close_all():
     for p in close_list[:]:
         pexpect_close(p)
 
-def start_SIL(atype, valgrind=False, wipe=False, CLI=False):
+def start_SIL(atype, valgrind=False, wipe=False, CLI=False, height=None):
     '''launch a SIL instance'''
     cmd=""
     if valgrind and os.path.exists('/usr/bin/valgrind'):
@@ -105,6 +105,8 @@ def start_SIL(atype, valgrind=False, wipe=False, CLI=False):
         cmd += ' -w'
     if CLI:
         cmd += ' -s'
+    if height is not None:
+        cmd += ' -H %u' % height
     ret = pexpect.spawn(cmd, logfile=sys.stdout, timeout=5)
     pexpect_autoclose(ret)
     ret.expect('Waiting for connection')