From 8c49690acf08bdbc7ae9af7e1f20c92cea247d91 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 11 Mar 2020 12:20:47 +1100
Subject: [PATCH] HAL_ChibiOS: implement in_expected_delay()

---
 libraries/AP_HAL_ChibiOS/Scheduler.cpp | 22 +++++++++++++++++-----
 libraries/AP_HAL_ChibiOS/Scheduler.h   |  6 ++++++
 2 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 90d7007482..5c1c5c2299 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -321,15 +321,27 @@ void Scheduler::_timer_thread(void *arg)
         // process any pending RC output requests
         hal.rcout->timer_tick();
 
-        if (sched->expect_delay_start != 0) {
-            uint32_t now = AP_HAL::millis();
-            if (now - sched->expect_delay_start <= sched->expect_delay_length) {
-                sched->watchdog_pat();
-            }
+        if (sched->in_expected_delay()) {
+            sched->watchdog_pat();
         }
     }
 }
 
+/*
+  return true if we are in a period of expected delay. This can be
+  used to suppress error messages
+*/
+bool Scheduler::in_expected_delay(void) const
+{
+    if (expect_delay_start != 0) {
+        uint32_t now = AP_HAL::millis();
+        if (now - expect_delay_start <= expect_delay_length) {
+            return true;
+        }
+    }
+    return false;
+}
+
 #ifndef HAL_NO_MONITOR_THREAD
 void Scheduler::_monitor_thread(void *arg)
 {
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h
index f09f2e761a..5a98860470 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.h
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.h
@@ -105,6 +105,12 @@ public:
      */
     void     expect_delay_ms(uint32_t ms) override;
 
+    /*
+      return true if we are in a period of expected delay. This can be
+      used to suppress error messages
+     */
+    bool in_expected_delay(void) const override;
+    
     /*
       disable interrupts and return a context that can be used to
       restore the interrupt state. This can be used to protect