diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index b676f5482d..e8d602e75b 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -540,10 +540,24 @@ void Scheduler::expect_delay_ms(uint32_t ms)
         return;
     }
     if (ms == 0) {
-        expect_delay_start = 0;
+        if (expect_delay_nesting > 0) {
+            expect_delay_nesting--;
+        }
+        if (expect_delay_nesting == 0) {
+            expect_delay_start = 0;
+        }
     } else {
-        expect_delay_start = AP_HAL::millis();
+        uint32_t now = AP_HAL::millis();
+        if (expect_delay_start != 0) {
+            // we already have a delay running, possibly extend it
+            uint32_t done = now - expect_delay_start;
+            if (expect_delay_length > done) {
+                ms = MAX(ms, expect_delay_length - done);
+            }
+        }
+        expect_delay_start = now;
         expect_delay_length = ms;
+        expect_delay_nesting++;
 
         // also put our priority below timer thread if we are boosted
         boost_end();
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h
index 9fc9b22f50..6371be7033 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.h
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.h
@@ -133,6 +133,7 @@ private:
     bool _priority_boosted;
     uint32_t expect_delay_start;
     uint32_t expect_delay_length;
+    uint32_t expect_delay_nesting;
 
     AP_HAL::MemberProc _timer_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS];
     uint8_t _num_timer_procs;