mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
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:
parent
b217771dbf
commit
d7a0eb42ee
@ -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;
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user