diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 0dd0eaff89..9e7a6ce127 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -190,8 +190,7 @@ static THD_FUNCTION(main_loop,arg) chThdSetPriority(APM_MAIN_PRIORITY); // setup watchdog to reset if main loop stops - bool using_watchdog = AP_BoardConfig::watchdog_enabled(); - if (using_watchdog) { + if (AP_BoardConfig::watchdog_enabled()) { stm32_watchdog_init(); } @@ -209,9 +208,7 @@ static THD_FUNCTION(main_loop,arg) if (!schedulerInstance.check_called_boost()) { hal.scheduler->delay_microseconds(250); } - if (using_watchdog) { - stm32_watchdog_pat(); - } + stm32_watchdog_pat(); #if 0 // simple method to test watchdog functionality diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index ca260bcc8b..03bd5d5dae 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -33,6 +33,7 @@ #include #include #include "hwdef/common/stm32_util.h" +#include "hwdef/common/watchdog.h" #include "shared_dma.h" #include "sdcard.h" @@ -289,6 +290,13 @@ 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) { + stm32_watchdog_pat(); + } + } } } #if HAL_WITH_UAVCAN @@ -473,4 +481,24 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_ return true; } +/* + inform the scheduler that we are calling an operation from the + main thread that may take an extended amount of time. This can + 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) +{ + if (!in_main_thread()) { + // only for main thread + return; + } + if (ms == 0) { + expect_delay_start = 0; + } else { + expect_delay_start = AP_HAL::millis(); + expect_delay_length = ms; + } +} + #endif // CH_CFG_USE_DYNAMIC diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 4e86a5189a..db83a3ab23 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -80,6 +80,14 @@ public: bool check_called_boost(void); + /* + inform the scheduler that we are calling an operation from the + main thread that may take an extended amount of time. This can + 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) override; + /* disable interrupts and return a context that can be used to restore the interrupt state. This can be used to protect @@ -103,7 +111,8 @@ private: AP_HAL::Proc _failsafe; bool _called_boost; bool _priority_boosted; - + uint32_t expect_delay_start; + uint32_t expect_delay_length; AP_HAL::MemberProc _timer_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS]; uint8_t _num_timer_procs; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c index fd3303af0c..6f41a1a57a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c @@ -67,6 +67,9 @@ void __late_init(void) { halInit(); chSysInit(); stm32_watchdog_save_reason(); +#ifndef HAL_BOOTLOADER_BUILD + stm32_watchdog_clear_reason(); +#endif #if CH_CFG_USE_HEAP == TRUE malloc_init(); #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index 10583fc7b3..2d77e71deb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c @@ -51,6 +51,9 @@ typedef struct #define IWDGD (*(IWDG_Regs *)(IWDG_BASE)) +static bool was_watchdog_reset; +static bool watchdog_enabled; + /* setup the watchdog */ @@ -61,6 +64,7 @@ void stm32_watchdog_init(void) IWDGD.PR = 8; IWDGD.RLR = 0xFFF; IWDGD.KR = 0xCCCC; + watchdog_enabled = true; } /* @@ -69,11 +73,11 @@ void stm32_watchdog_init(void) */ void stm32_watchdog_pat(void) { - IWDGD.KR = 0xAAAA; + if (watchdog_enabled) { + IWDGD.KR = 0xAAAA; + } } -static bool was_watchdog_reset; - /* save reason code for reset */ @@ -82,6 +86,13 @@ void stm32_watchdog_save_reason(void) if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) { was_watchdog_reset = true; } +} + +/* + clear reason code for reset + */ +void stm32_watchdog_clear_reason(void) +{ WDG_RESET_STATUS = WDG_RESET_CLEAR; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h index 3310959626..489bf40ccb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h @@ -23,6 +23,11 @@ bool stm32_was_watchdog_reset(void); save the reset reason code */ void stm32_watchdog_save_reason(void); + +/* + clear reset reason code + */ +void stm32_watchdog_clear_reason(void); #ifdef __cplusplus }