diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index ebc46bf691..fea3d0179f 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -204,7 +204,8 @@ void SITL_State::_fdm_input_step(void)
 void SITL_State::wait_clock(uint64_t wait_time_usec)
 {
     while (AP_HAL::micros64() < wait_time_usec) {
-        if (hal.scheduler->in_main_thread()) {
+        if (hal.scheduler->in_main_thread() ||
+            Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) {
             _fdm_input_step();
         } else {
             usleep(1000);
diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp
index 0f8543b9c6..5d687b911d 100644
--- a/libraries/AP_HAL_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_SITL/Scheduler.cpp
@@ -29,6 +29,8 @@ uint8_t Scheduler::_num_io_procs = 0;
 bool Scheduler::_in_io_proc = false;
 bool Scheduler::_should_reboot = false;
 
+bool Scheduler::_in_semaphore_take_wait = false;
+
 Scheduler::thread_attr *Scheduler::threads;
 HAL_Semaphore Scheduler::_thread_sem;
 
@@ -51,6 +53,27 @@ bool Scheduler::in_main_thread() const
     return false;
 }
 
+/*
+ * semaphore_wait_hack_required - possibly move time input step
+ * forward even if we are currently pretending to be the IO or timer
+ * threads.
+ *
+ * Without this, if another thread has taken a semaphore (e.g. the
+ * Object Avoidance thread), and an "IO process" tries to take that
+ * semaphore with a timeout specified, then we end up not advancing
+ * time (due to the logic in SITL_State::wait_clock) and thus taking
+ * the semaphore never times out - meaning we essentially deadlock.
+ */
+bool Scheduler::semaphore_wait_hack_required()
+{
+    if (pthread_self() != _main_ctx) {
+        // only the main thread ever moves stuff forwards
+        return false;
+    }
+
+    return _in_semaphore_take_wait;
+}
+
 void Scheduler::delay_microseconds(uint16_t usec)
 {
     uint64_t start = AP_HAL::micros64();
diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h
index fcb49c5207..a03f759fe3 100644
--- a/libraries/AP_HAL_SITL/Scheduler.h
+++ b/libraries/AP_HAL_SITL/Scheduler.h
@@ -56,7 +56,16 @@ public:
      */
     bool thread_create(AP_HAL::MemberProc, const char *name,
                        uint32_t stack_size, priority_base base, int8_t priority) override;
-    
+
+    void set_in_semaphore_take_wait(bool value) { _in_semaphore_take_wait = value; }
+    /*
+     * semaphore_wait_hack_required - possibly move time input step
+     * forward even if we are currently pretending to be the IO or timer
+     * threads.
+     */
+    // a couple of helper functions to cope with SITL's time stepping
+    bool semaphore_wait_hack_required();
+
 private:
     SITL_State *_sitlState;
     uint8_t _nested_atomic_ctr;
@@ -72,6 +81,10 @@ private:
     static bool _in_timer_proc;
     static bool _in_io_proc;
 
+    // boolean set by the Semaphore code to indicate it's currently
+    // waiting for a take-timeout to occur.
+    static bool _in_semaphore_take_wait;
+
     void stop_clock(uint64_t time_usec) override;
 
     static void *thread_create_trampoline(void *ctx);
diff --git a/libraries/AP_HAL_SITL/Semaphores.cpp b/libraries/AP_HAL_SITL/Semaphores.cpp
index 78ede0c394..48e964a778 100644
--- a/libraries/AP_HAL_SITL/Semaphores.cpp
+++ b/libraries/AP_HAL_SITL/Semaphores.cpp
@@ -3,6 +3,7 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 #include "Semaphores.h"
+#include "Scheduler.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -42,7 +43,9 @@ bool Semaphore::take(uint32_t timeout_ms)
     }
     uint64_t start = AP_HAL::micros64();
     do {
+        Scheduler::from(hal.scheduler)->set_in_semaphore_take_wait(true);
         hal.scheduler->delay_microseconds(200);
+        Scheduler::from(hal.scheduler)->set_in_semaphore_take_wait(false);
         if (take_nonblocking()) {
             return true;
         }