From 0c440d567584f6ab574a5a26a050de960de443f0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 6 Dec 2020 10:29:31 +1100
Subject: [PATCH] HAL_ChibiOS: cope with flash erase in expected delay

when there has been a flash erase when we are definately in an
expected delay
---
 libraries/AP_HAL_ChibiOS/Scheduler.cpp        |  8 +++++++-
 libraries/AP_HAL_ChibiOS/hwdef/common/flash.c | 16 ++++++++++++++++
 libraries/AP_HAL_ChibiOS/hwdef/common/flash.h |  1 +
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 7e277a317a..06c1a13ebf 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -35,6 +35,7 @@
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include "hwdef/common/stm32_util.h"
+#include "hwdef/common/flash.h"
 #include "hwdef/common/watchdog.h"
 #include <AP_Filesystem/AP_Filesystem.h>
 #include "shared_dma.h"
@@ -344,6 +345,11 @@ bool Scheduler::in_expected_delay(void) const
             return true;
         }
     }
+#ifndef HAL_NO_FLASH_SUPPORT
+    if (stm32_flash_recent_erase()) {
+        return true;
+    }
+#endif
     return false;
 }
 
@@ -393,7 +399,7 @@ void Scheduler::_monitor_thread(void *arg)
                 }
 #endif
         }
-        if (loop_delay >= 500) {
+        if (loop_delay >= 500 && !sched->in_expected_delay()) {
             // at 500ms we declare an internal error
             INTERNAL_ERROR(AP_InternalError::error_t::main_loop_stuck);
         }
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
index f21dc950e1..48e800fbdd 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
@@ -52,6 +52,7 @@
 #include "hal.h"
 #include <string.h>
 #include "stm32_util.h"
+#include "hrt.h"
 
 #include <assert.h>
 
@@ -334,6 +335,8 @@ bool stm32_flash_ispageerased(uint32_t page)
     return true;
 }
 
+static uint32_t last_erase_ms;
+
 /*
   erase a page
  */
@@ -343,6 +346,8 @@ bool stm32_flash_erasepage(uint32_t page)
         return false;
     }
 
+    last_erase_ms = hrt_millis32();
+
 #if STM32_FLASH_DISABLE_ISR
     syssts_t sts = chSysGetStatusAndLockX();
 #endif
@@ -401,6 +406,9 @@ bool stm32_flash_erasepage(uint32_t page)
 #if STM32_FLASH_DISABLE_ISR
     chSysRestoreStatusX(sts);
 #endif
+
+    last_erase_ms = hrt_millis32();
+
     return stm32_flash_ispageerased(page);
 }
 
@@ -655,5 +663,13 @@ void stm32_flash_keep_unlocked(bool set)
     }
 }
 
+/*
+  return true if we had a recent erase
+ */
+bool stm32_flash_recent_erase(void)
+{
+    return hrt_millis32() - last_erase_ms < 3000U;
+}
+
 #endif // HAL_NO_FLASH_SUPPORT
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
index 9e6c7ef5fd..8c440a78af 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
@@ -27,6 +27,7 @@ bool stm32_flash_erasepage(uint32_t page);
 bool stm32_flash_write(uint32_t addr, const void *buf, uint32_t count);
 void stm32_flash_keep_unlocked(bool set);
 bool stm32_flash_ispageerased(uint32_t page);
+bool stm32_flash_recent_erase(void);
 #ifdef __cplusplus
 }
 #endif