diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index 2d0191f46c98c..e6720cea2eb10 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -102,8 +102,14 @@ def test_alt_hold(self):
         self.mavproxy.send('mode ALT_HOLD\n')
         self.wait_mode('ALT_HOLD')
 
-
-        self.set_rc(Joystick.Throttle, 1000)
+        msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
+        if msg is None:
+            raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
+        pwm = 1000
+        if msg.relative_alt/1000.0 < -5.5:
+            # need to g`o up, not down!
+            pwm = 2000
+        self.set_rc(Joystick.Throttle, pwm)
         self.wait_altitude(altitude_min=-6, altitude_max=-5)
         self.set_rc(Joystick.Throttle, 1500)
 
@@ -191,8 +197,7 @@ def dive_mission(self, filename):
 
         self.arm_vehicle()
 
-        self.mavproxy.send('mode auto\n')
-        self.wait_mode('AUTO')
+        self.change_mode('AUTO')
 
         self.wait_waypoint(1, 5, max_dist=5)
 
@@ -214,8 +219,7 @@ def test_gripper_mission(self):
             self.mavproxy.send('mode loiter\n')
             self.wait_ready_to_arm()
             self.arm_vehicle()
-            self.mavproxy.send('mode auto\n')
-            self.wait_mode('AUTO')
+            self.change_mode('AUTO')
             self.mavproxy.expect("Gripper Grabbed")
             self.mavproxy.expect("Gripper Released")
         except Exception as e:
diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 85448183c9e68..183642473b69e 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -212,6 +212,20 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
             usleep(1000);
         }
     }
+    // check the outbound TCP queue size.  If it is too long then
+    // MAVProxy/pymavlink take too long to process packets and it ends
+    // up seeing traffic well into our past and hits time-out
+    // conditions.
+    if (sitl_model->get_speedup() > 1) {
+        while (true) {
+            const int queue_length = ((HALSITL::UARTDriver*)hal.uartA)->get_system_outqueue_length();
+            // ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length);
+            if (queue_length < 1024) {
+                break;
+            }
+            usleep(1000);
+        }
+    }
 }
 
 #define streq(a, b) (!strcmp(a, b))
diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
index 7485c58c63fce..8af669e66ed5a 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.cpp
+++ b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -770,5 +770,25 @@ uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes)
     return last_receive_us;
 }
 
+ssize_t UARTDriver::get_system_outqueue_length() const
+{
+    if (!_connected) {
+        return 0;
+    }
+
+#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
+    return 0;
+#elif defined(__APPLE__) && defined(__MACH__)
+    return 0;
+#else
+    int size;
+    if (ioctl(_fd, TIOCOUTQ, &size) == -1) {
+        // ::fprintf(stderr, "ioctl TIOCOUTQ failed: %m\n");
+        return 0;
+    }
+    return size;
+#endif
+}
+
 #endif // CONFIG_HAL_BOARD
 
diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h
index cd5a4e4f72261..6c70b4080aee6 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.h
+++ b/libraries/AP_HAL_SITL/UARTDriver.h
@@ -33,6 +33,8 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
         return true;
     }
 
+    ssize_t get_system_outqueue_length() const;
+
     void set_blocking_writes(bool blocking) override
     {
         _nonblocking_writes = !blocking;
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 2e5f4248ad161..6f5ef664b5e6a 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -47,6 +47,7 @@ class Aircraft {
       set simulation speedup
      */
     void set_speedup(float speedup);
+    float get_speedup() { return target_speedup; }
 
     /*
       set instance number