diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 976dbc5834..b952d7ae24 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -608,7 +608,9 @@ void AP_Periph_FW::prepare_reboot() // delay to give the ACK a chance to get out, the LEDs to flash, // the IO board safety to be forced on, the parameters to flush, + hal.scheduler->expect_delay_ms(100); hal.scheduler->delay(40); + hal.scheduler->expect_delay_ms(0); } /* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index c1a5eeb049..689f9e4acc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c @@ -22,6 +22,16 @@ #error "Unsupported IWDG RCC MCU config" #endif +/* + define for controlling how long the watchdog is set for. +*/ +#ifndef STM32_WDG_TIMEOUT_MS +#define STM32_WDG_TIMEOUT_MS 2048 +#endif +#if STM32_WDG_TIMEOUT_MS > 4096 || STM32_WDG_TIMEOUT_MS < 20 +#error "Watchdog timeout out of range" +#endif + /* defines for working out if the reset was from the watchdog */ @@ -68,16 +78,17 @@ static bool watchdog_enabled; */ void stm32_watchdog_init(void) { - // setup for 2s reset + // setup the watchdog timeout + // t = 4 * 2^PR * (RLR+1) / 32KHz IWDGD.KR = 0x5555; - IWDGD.PR = 2; // div16 - IWDGD.RLR = 0xFFF; + IWDGD.PR = 3; // changing this would change the definition of STM32_WDG_TIMEOUT_MS + IWDGD.RLR = STM32_WDG_TIMEOUT_MS - 1; IWDGD.KR = 0xCCCC; watchdog_enabled = true; } /* - pat the dog, to prevent a reset. If not called for 1s + pat the dog, to prevent a reset. If not called for STM32_WDG_TIMEOUT_MS after stm32_watchdog_init() then MCU will reset */ void stm32_watchdog_pat(void) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h index f015dee9c1..de8e394149 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h @@ -10,7 +10,7 @@ extern "C" { void stm32_watchdog_init(void); /* - pat the dog, to prevent a reset. If not called for 1s + pat the dog, to prevent a reset. If not called for STM32_WDG_TIMEOUT_MS after stm32_watchdog_init() then MCU will reset */ void stm32_watchdog_pat(void);