From d7a0eb42eeff3a8bf88f8e49090bea14cf7e46a1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 9 May 2019 21:15:58 +1000 Subject: [PATCH] HAL_ChibiOS: added logging of watchdog data log MON msgs in the leadup to a watchdog, and log a WDOG message after a watchdog reset --- .../AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 41 ++++++------ libraries/AP_HAL_ChibiOS/Scheduler.cpp | 63 +++++++++++++++++-- libraries/AP_HAL_ChibiOS/Scheduler.h | 13 +++- .../AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat | 1 + 4 files changed, 90 insertions(+), 28 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index ddbfc692c1..88c8864a2f 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -29,6 +29,7 @@ #include "hwdef/common/watchdog.h" #include #include +#include #include @@ -191,23 +192,14 @@ static void main_loop() */ hal_chibios_set_priority(APM_STARTUP_PRIORITY); - schedulerInstance.hal_initialized(); - if (stm32_was_watchdog_reset()) { // load saved watchdog data stm32_watchdog_load((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4); } - g_callbacks->setup(); - hal.scheduler->system_initialized(); + schedulerInstance.hal_initialized(); - thread_running = true; - chRegSetThreadName(SKETCHNAME); - - /* - switch to high priority for main loop - */ - chThdSetPriority(APM_MAIN_PRIORITY); + g_callbacks->setup(); #ifndef IOMCU_FW // setup watchdog to reset if main loop stops @@ -217,12 +209,27 @@ static void main_loop() if (hal.util->was_watchdog_reset()) { AP::internalerror().error(AP_InternalError::error_t::watchdog_reset); + AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; + AP::logger().Write("WDOG", "TimeUS,Task,IErr,IErrCnt", "QbII", + AP_HAL::micros64(), + pd.scheduler_task, + pd.internal_errors, + pd.internal_error_count); } #else stm32_watchdog_init(); #endif + schedulerInstance.watchdog_pat(); - uint32_t last_watchdog_save = AP_HAL::millis(); + hal.scheduler->system_initialized(); + + thread_running = true; + chRegSetThreadName(SKETCHNAME); + + /* + switch to high priority for main loop + */ + chThdSetPriority(APM_MAIN_PRIORITY); while (true) { g_callbacks->loop(); @@ -240,15 +247,7 @@ static void main_loop() hal.scheduler->delay_microseconds(50); } #endif - stm32_watchdog_pat(); - - uint32_t now = AP_HAL::millis(); - if (now - last_watchdog_save >= 100) { - // save persistent data every 100ms - last_watchdog_save = now; - stm32_watchdog_save((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4); - } - + schedulerInstance.watchdog_pat(); } thread_running = false; } diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 8497ad9a8c..b934bed4eb 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #if CH_CFG_USE_DYNAMIC == TRUE @@ -57,6 +58,10 @@ THD_WORKING_AREA(_io_thread_wa, IO_THD_WA_SIZE); #ifndef HAL_USE_EMPTY_STORAGE THD_WORKING_AREA(_storage_thread_wa, STORAGE_THD_WA_SIZE); #endif +#ifndef HAL_NO_MONITOR_THREAD +THD_WORKING_AREA(_monitor_thread_wa, MONITOR_THD_WA_SIZE); +#endif + Scheduler::Scheduler() { } @@ -66,6 +71,15 @@ void Scheduler::init() chBSemObjectInit(&_timer_semaphore, false); chBSemObjectInit(&_io_semaphore, false); +#ifndef HAL_NO_MONITOR_THREAD + // setup the monitor thread - this is used to detect software lockups + _monitor_thread_ctx = chThdCreateStatic(_monitor_thread_wa, + sizeof(_monitor_thread_wa), + APM_MONITOR_PRIORITY, /* Initial priority. */ + _monitor_thread, /* Thread function. */ + this); /* Thread parameter. */ +#endif + #ifndef HAL_NO_TIMER_THREAD // setup the timer thread - this will call tasks at 1kHz _timer_thread_ctx = chThdCreateStatic(_timer_thread_wa, @@ -310,12 +324,47 @@ void Scheduler::_timer_thread(void *arg) 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(); + sched->watchdog_pat(); } } } } +void Scheduler::_monitor_thread(void *arg) +{ + Scheduler *sched = (Scheduler *)arg; + chRegSetThreadName("apm_monitor"); + + while (!sched->_initialized) { + sched->delay(100); + } + bool using_watchdog = AP_BoardConfig::watchdog_enabled(); + + while (true) { + sched->delay(100); + if (using_watchdog) { + stm32_watchdog_save((uint32_t *)&hal.util->persistent_data, (sizeof(hal.util->persistent_data)+3)/4); + } + uint32_t now = AP_HAL::millis(); + uint32_t loop_delay = now - sched->last_watchdog_pat_ms; + if (loop_delay >= 200) { + // the main loop has been stuck for at least + // 200ms. Starting logging the main loop state + AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; + AP::logger().Write("MON", "TimeUS,LDelay,Task,IErr,IErrCnt", "QIbII", + AP_HAL::micros64(), + loop_delay, + pd.scheduler_task, + pd.internal_errors, + pd.internal_error_count); + } + if (loop_delay >= 500) { + // at 500ms we declare an internal error + AP::internalerror().error(AP_InternalError::error_t::main_loop_stuck); + } + } +} + void Scheduler::_rcin_thread(void *arg) { Scheduler *sched = (Scheduler *)arg; @@ -391,11 +440,6 @@ void Scheduler::_storage_thread(void* arg) } } -bool Scheduler::in_main_thread() const -{ - return get_main_thread() == chThdGetSelfX(); -} - void Scheduler::system_initialized() { if (_initialized) { @@ -503,4 +547,11 @@ void Scheduler::expect_delay_ms(uint32_t ms) } } +// pat the watchdog +void Scheduler::watchdog_pat(void) +{ + stm32_watchdog_pat(); + last_watchdog_pat_ms = AP_HAL::millis(); +} + #endif // CH_CFG_USE_DYNAMIC diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index d5ea7faa5e..9fc9b22f50 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -22,6 +22,7 @@ #define CHIBIOS_SCHEDULER_MAX_TIMER_PROCS 8 +#define APM_MONITOR_PRIORITY 183 #define APM_MAIN_PRIORITY 180 #define APM_TIMER_PRIORITY 181 #define APM_RCIN_PRIORITY 177 @@ -68,6 +69,9 @@ #define STORAGE_THD_WA_SIZE 2048 #endif +#ifndef MONITOR_THD_WA_SIZE +#define MONITOR_THD_WA_SIZE 512 +#endif /* Scheduler implementation: */ class ChibiOS::Scheduler : public AP_HAL::Scheduler { @@ -86,7 +90,8 @@ public: void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override; void reboot(bool hold_in_bootloader) override; - bool in_main_thread() const override; + bool in_main_thread() const override { return get_main_thread() == chThdGetSelfX(); } + void system_initialized() override; void hal_initialized() { _hal_initialized = true; } @@ -117,6 +122,9 @@ public: */ bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override; + // pat the watchdog + void watchdog_pat(void); + private: bool _initialized; volatile bool _hal_initialized; @@ -133,11 +141,13 @@ private: AP_HAL::MemberProc _io_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS]; uint8_t _num_io_procs; volatile bool _in_io_proc; + uint32_t last_watchdog_pat_ms; thread_t* _timer_thread_ctx; thread_t* _rcin_thread_ctx; thread_t* _io_thread_ctx; thread_t* _storage_thread_ctx; + thread_t* _monitor_thread_ctx; #if CH_CFG_USE_SEMAPHORES == TRUE binary_semaphore_t _timer_semaphore; @@ -148,6 +158,7 @@ private: static void _io_thread(void *arg); static void _storage_thread(void *arg); static void _uart_thread(void *arg); + static void _monitor_thread(void *arg); void _run_timers(); void _run_io(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat index 0fd23cb98d..e8ebfdcdce 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat @@ -120,6 +120,7 @@ define PORT_INT_REQUIRED_STACK 64 # avoid timer and RCIN threads to save memory define HAL_NO_TIMER_THREAD define HAL_NO_RCIN_THREAD +define HAL_NO_MONITOR_THREAD #defined to turn off undef warnings define __FPU_PRESENT 0