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
This commit is contained in:
Andrew Tridgell 2019-05-09 21:15:58 +10:00
parent b217771dbf
commit d7a0eb42ee
4 changed files with 90 additions and 28 deletions

View File

@ -29,6 +29,7 @@
#include "hwdef/common/watchdog.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Logger/AP_Logger.h>
#include <hwdef.h>
@ -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;
}

View File

@ -26,6 +26,7 @@
#include <AP_HAL_ChibiOS/RCOutput.h>
#include <AP_HAL_ChibiOS/RCInput.h>
#include <AP_HAL_ChibiOS/CAN.h>
#include <AP_InternalError/AP_InternalError.h>
#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

View File

@ -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);

View File

@ -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