diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp
index 981e0bf92b..b3902f8500 100644
--- a/libraries/AP_HAL_Linux/Scheduler.cpp
+++ b/libraries/AP_HAL_Linux/Scheduler.cpp
@@ -83,6 +83,8 @@ void Scheduler::init()
         SCHED_THREAD(io, IO),
     };
 
+    _main_ctx = pthread_self();
+
 #if !APM_BUILD_TYPE(APM_BUILD_Replay)
     // we don't run Replay in real-time...
     mlockall(MCL_CURRENT|MCL_FUTURE);
@@ -148,6 +150,12 @@ void Scheduler::delay(uint16_t ms)
     if (_stopped_clock_usec) {
         return;
     }
+
+    if (!in_main_thread()) {
+        fprintf(stderr, "Scheduler::delay() called outside main thread\n");
+        return;
+    }
+
     uint64_t start = AP_HAL::millis64();
 
     while ((AP_HAL::millis64() - start) < ms) {
@@ -345,6 +353,11 @@ bool Scheduler::in_timerprocess()
     return _in_timer_proc;
 }
 
+bool Scheduler::in_main_thread()
+{
+    return pthread_equal(pthread_self(), _main_ctx);
+}
+
 void Scheduler::_wait_all_threads()
 {
     int r = pthread_barrier_wait(&_initialized_barrier);
diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h
index ec37e50ff6..645e234e12 100644
--- a/libraries/AP_HAL_Linux/Scheduler.h
+++ b/libraries/AP_HAL_Linux/Scheduler.h
@@ -36,6 +36,7 @@ public:
     void     resume_timer_procs();
 
     bool     in_timerprocess();
+    bool     in_main_thread();
 
     void     register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
 
@@ -101,6 +102,7 @@ private:
 
     uint64_t _stopped_clock_usec;
     uint64_t _last_stack_debug_msec;
+    pthread_t _main_ctx;
 
     Semaphore _timer_semaphore;
     Semaphore _io_semaphore;