HAL_ChibiOS: cope with nesting of expected_delay_ms()

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();

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;