diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 16068e35de..87996b2e5d 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -27,6 +27,7 @@ #include "hwdef/common/usbcfg.h" #include "hwdef/common/stm32_util.h" #include "hwdef/common/watchdog.h" +#include #include @@ -189,7 +190,10 @@ static THD_FUNCTION(main_loop,arg) chThdSetPriority(APM_MAIN_PRIORITY); // setup watchdog to reset if main loop stops - stm32_watchdog_init(); + bool using_watchdog = AP_BoardConfig::watchdog_enabled(); + if (using_watchdog) { + stm32_watchdog_init(); + } while (true) { g_callbacks->loop(); @@ -205,7 +209,9 @@ static THD_FUNCTION(main_loop,arg) if (!schedulerInstance.check_called_boost()) { hal.scheduler->delay_microseconds(250); } - stm32_watchdog_pat(); + if (using_watchdog) { + stm32_watchdog_pat(); + } } thread_running = false; }