HAL_ChibiOS: expect a long delay on flash page erase

this prevents the display of errors on flash page erase

libraries/AP_HAL_ChibiOS/Scheduler.cpp

@@ -551,7 +551,7 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_
   be used to prevent watchdog reset during expected long delays
   A value of zero cancels the previous expected delay
 */
-void Scheduler::expect_delay_ms(uint32_t ms)
+void Scheduler::_expect_delay_ms(uint32_t ms)
 {
     if (!in_main_thread()) {
         // only for main thread
@@ -561,6 +561,8 @@ void Scheduler::expect_delay_ms(uint32_t ms)
     // pat once immediately
     watchdog_pat();
 
+    WITH_SEMAPHORE(expect_delay_sem);
+
     if (ms == 0) {
         if (expect_delay_nesting > 0) {
             expect_delay_nesting--;
@@ -586,6 +588,18 @@ void Scheduler::expect_delay_ms(uint32_t ms)
     }
 }
 
+/*
+  this is _expect_delay_ms() with check that we are in the main thread
+ */
+void Scheduler::expect_delay_ms(uint32_t ms)
+{
+    if (!in_main_thread()) {
+        // only for main thread
+        return;
+    }
+    _expect_delay_ms(ms);
+}
+
 // pat the watchdog
 void Scheduler::watchdog_pat(void)
 {

libraries/AP_HAL_ChibiOS/Scheduler.h

@@ -103,6 +103,7 @@ public:
       be used to prevent watchdog reset during expected long delays
       A value of zero cancels the previous expected delay
      */
+    void     _expect_delay_ms(uint32_t ms);
     void     expect_delay_ms(uint32_t ms) override;
 
     /*
@@ -140,6 +141,7 @@ private:
     uint32_t expect_delay_start;
     uint32_t expect_delay_length;
     uint32_t expect_delay_nesting;
+    HAL_Semaphore expect_delay_sem;
 
     AP_HAL::MemberProc _timer_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS];
     uint8_t _num_timer_procs;

libraries/AP_HAL_ChibiOS/Storage.cpp

@@ -18,6 +18,8 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 
 #include "Storage.h"
+#include "HAL_ChibiOS_Class.h"
+#include "Scheduler.h"
 #include "hwdef/common/flash.h"
 #include <AP_Filesystem/AP_Filesystem.h>
 #include "sdcard.h"
@@ -314,10 +316,22 @@ bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, u
 bool Storage::_flash_erase_sector(uint8_t sector)
 {
 #ifdef STORAGE_FLASH_PAGE
+    // erasing a page can take long enough that USB may not initialise properly if it happens
+    // while the host is connecting. Only do a flash erase if we have been up for more than 4s
     for (uint8_t i=0; i<STORAGE_FLASH_RETRIES; i++) {
+        /*
+          a sector erase stops the whole MCU. We need to setup a long
+          expected delay, and not only when running in the main
+          thread.  We can't use EXPECT_DELAY_MS() as it checks we are
+          in the main thread
+         */
+        ChibiOS::Scheduler *sched = (ChibiOS::Scheduler *)hal.scheduler;
+        sched->_expect_delay_ms(1000);
         if (hal.flash->erasepage(_flash_page+sector)) {
+            sched->_expect_delay_ms(0);
             return true;
         }
+        sched->_expect_delay_ms(0);
         hal.scheduler->delay(1);
     }
     return false;