diff --git a/boards/auterion/fmu-v6s/default.px4board b/boards/auterion/fmu-v6s/default.px4board index 2a4cbbe8a4..514794a521 100644 --- a/boards/auterion/fmu-v6s/default.px4board +++ b/boards/auterion/fmu-v6s/default.px4board @@ -55,6 +55,7 @@ CONFIG_MODULES_HARDFAULT_STREAM=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_TASK_WATCHDOG=y CONFIG_MODULES_LOGGER=y CONFIG_LOGGER_STACK_SIZE=4100 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y diff --git a/boards/auterion/fmu-v6s/init/rc.board_defaults b/boards/auterion/fmu-v6s/init/rc.board_defaults index 1efbfc3811..20cd5a1afe 100644 --- a/boards/auterion/fmu-v6s/init/rc.board_defaults +++ b/boards/auterion/fmu-v6s/init/rc.board_defaults @@ -34,5 +34,8 @@ nshterm /dev/ttyS3 & # Start the time_persistor to cyclically store the RTC in FRAM time_persistor start +# Start the task_watchdog as we do not have the logger watchdog +task_watchdog start + # Start the ESC telemetry dshot telemetry -d /dev/ttyS5 -x diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp index 89ba247ed2..6766b5376c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp @@ -126,6 +126,57 @@ void SdCardChecks::checkAndReport(const Context &context, Report &reporter) } } +#if CONFIG_MODULES_TASK_WATCHDOG + // Check for task watchdog dump files + + if (!_watchdog_checked_once && _param_com_arm_hardfault_check.get()) { + _watchdog_checked_once = true; + + DIR *wdp = opendir(PX4_STORAGEDIR "/task_watchdog"); + + if (wdp != nullptr) { + + struct dirent *wresult; + + while ((wresult = readdir(wdp)) && !_watchdog_file_present) { + + // Check for pattern wdg_*.log or load_*.log + const size_t len = strlen(wresult->d_name); + + if (len > 5 && strcmp(wresult->d_name + len - 4, ".log") == 0) { + if (strncmp("wdg_", wresult->d_name, 4) == 0 + || strncmp("load_", wresult->d_name, 5) == 0) { + _watchdog_file_present = true; + } + } + } + + closedir(wdp); + } + } + + if (_watchdog_file_present && _param_com_arm_hardfault_check.get()) { + /* EVENT + * @description + * The SD card contains task watchdog dump files from a previous task starvation event. + * + * + * Remove the files in the 'task_watchdog' directory on the SD card after analysis. + * + * This check can be configured via COM_ARM_HFLT_CHK parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, + events::ID("check_task_watchdog_present"), + events::Log::Error, "Task watchdog dumps present on SD card"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Task watchdog dumps present on SD"); + } + } + +#endif /* CONFIG_MODULES_TASK_WATCHDOG */ + #endif /* __PX4_NUTTX */ #endif /* PX4_STORAGEDIR */ } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp index da02ee8292..b8a0ac70bb 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp @@ -50,6 +50,8 @@ private: #ifdef __PX4_NUTTX bool _hardfault_checked_once {false}; bool _hardfault_file_present {false}; + bool _watchdog_checked_once {false}; + bool _watchdog_file_present {false}; #endif #endif diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ed762fd40c..c123219f4f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -812,9 +812,9 @@ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1); /** - * Enable FMU SD card hardfault detection check + * Enable FMU SD card hardfault / watchdog detection check * - * This check detects if there are hardfault files present on the + * This check detects if there are hardfault / watchdog files present on the * SD card. If so, and the parameter is enabled, arming is prevented. * * @group Commander diff --git a/src/modules/task_watchdog/CMakeLists.txt b/src/modules/task_watchdog/CMakeLists.txt new file mode 100644 index 0000000000..cb47379c87 --- /dev/null +++ b/src/modules/task_watchdog/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2026 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__task_watchdog + MAIN task_watchdog + SRCS + TaskWatchdog.cpp + DEPENDS + version +) diff --git a/src/modules/task_watchdog/Kconfig b/src/modules/task_watchdog/Kconfig new file mode 100644 index 0000000000..94e1933878 --- /dev/null +++ b/src/modules/task_watchdog/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_TASK_WATCHDOG + bool "task_watchdog" + default n + depends on PLATFORM_NUTTX + ---help--- + Detect tasks running too long and dump their register/stack state and cpuload to files diff --git a/src/modules/task_watchdog/TaskWatchdog.cpp b/src/modules/task_watchdog/TaskWatchdog.cpp new file mode 100644 index 0000000000..3d91f43616 --- /dev/null +++ b/src/modules/task_watchdog/TaskWatchdog.cpp @@ -0,0 +1,486 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "TaskWatchdog.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#ifndef CONFIG_BUILD_FLAT +# error Task Watchdog requires flat build, please enable it to use this module +#endif + +using namespace time_literals; + +namespace task_watchdog +{ + +ModuleBase::Descriptor TaskWatchdog::desc{task_spawn, custom_command, print_usage}; + +TaskWatchdog::TaskWatchdog() +{ +} + +TaskWatchdog::~TaskWatchdog() +{ + hrt_cancel(&_hrt_call); + px4_sem_destroy(&_sem); +} + +int TaskWatchdog::run_trampoline(int argc, char *argv[]) +{ + return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * { + TaskWatchdog *instance = new TaskWatchdog(); + + if (instance == nullptr) + { + PX4_ERR("alloc failed"); + } + + return instance; + }, argc, argv); +} + +int TaskWatchdog::task_spawn(int argc, char *argv[]) +{ + desc.task_id = px4_task_spawn_cmd("task_watchdog", + SCHED_DEFAULT, + SCHED_PRIORITY_LOG_WRITER, + PX4_STACK_ADJUSTED(3250), + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (desc.task_id < 0) { + desc.task_id = -1; + PX4_ERR("task start failed: %d", errno); + return -errno; + } + + return 0; +} + +void TaskWatchdog::start() +{ + px4_sem_init(&_sem, 0, 0); + px4_sem_setprotocol(&_sem, SEM_PRIO_NONE); + _shared.sem = &_sem; + + // Find our own task in the system_load table + const pid_t my_pid = getpid(); + + sched_lock(); + + for (int i = 0; i < CONFIG_FS_PROCFS_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && system_load.tasks[i].tcb->pid == my_pid) { + _shared.monitored_task_index = i; + break; + } + } + + sched_unlock(); + + if (_shared.monitored_task_index < 0) { + PX4_ERR("could not find own task in system_load"); + } + + _shared.ready_to_run_timestamp = hrt_absolute_time(); + + // Start HRT ISR + hrt_call_every(&_hrt_call, 400_ms, 400_ms, isr_callback, &_shared); +} + +void TaskWatchdog::isr_callback(void *arg) +{ + watchdog_shared_s *shared = static_cast(arg); + + if (!system_load.initialized || shared->monitored_task_index < 0) { + return; + } + + const system_load_taskinfo_s &watchdog_task = system_load.tasks[shared->monitored_task_index]; + const hrt_abstime now = hrt_absolute_time(); + + if (!watchdog_task.valid) { + shared->monitored_task_index = -1; + return; + } + + // Already triggered — wait for the task side to process + if (shared->triggered.load()) { + // Wake the task, otherwise it can get stuck when starved before entering the sem wait + px4_sem_post(shared->sem); + return; + } + + // Already in post-trigger priority restore phase + if (shared->trigger_time != 0 && now > shared->trigger_time + 1500_ms) { + // Restore our original priority + sched_param param{}; + param.sched_priority = shared->original_priority; + sched_setparam(watchdog_task.tcb->pid, ¶m); + + // Only trigger once as the system may be in an unstable state + shared->monitored_task_index = -1; + return; + } + + // If the task ran recently, update the timestamp + if (watchdog_task.curr_start_time > shared->ready_to_run_timestamp) { + shared->ready_to_run_timestamp = watchdog_task.curr_start_time; + } + + // Reset timestamp on state transitions into/out of READYTORUN + uint8_t current_state = watchdog_task.tcb->task_state; + + if (current_state != TSTATE_TASK_READYTORUN || + (shared->last_state != TSTATE_TASK_READYTORUN && current_state == TSTATE_TASK_READYTORUN)) { + shared->ready_to_run_timestamp = now; + } + + shared->last_state = current_state; + + // Check if we've been starved for too long + if (!shared->manual_trigger && now <= shared->ready_to_run_timestamp + TRIGGER_THRESHOLD) { + // Wake the task so it runs briefly, keeping curr_start_time fresh + px4_sem_post(shared->sem); + return; + } + + // --- TRIGGERED --- + + shared->manual_trigger = false; + + // Boost our own priority so we actually get to run and write the dump + sched_param param{}; + sched_getparam(watchdog_task.tcb->pid, ¶m); + shared->original_priority = param.sched_priority; + param.sched_priority = SCHED_PRIORITY_LOG_WATCHDOG; + sched_setparam(watchdog_task.tcb->pid, ¶m); + + shared->trigger_time = now; + shared->triggered.store(true); + px4_sem_post(shared->sem); +} + +void TaskWatchdog::run() +{ + start(); + + while (!should_exit()) { + + if (_cpuload_pending) { + _cpuload_pending = false; + cpuload_monitor_stop(); + write_cpuload_file(); + } + + if (_shared.triggered.load()) { + _shared.triggered.store(false); + + PX4_ERR("task starvation detected!"); + capture_and_write_dump(); + + cpuload_monitor_start(); + init_print_load(&_load); + _cpuload_pending = true; + } + + if (_cpuload_pending) { + px4_usleep(CPULOAD_ACCUMULATE_TIME); + + } else { + // Drain any extra posts that accumulated while the task was asleep + int val; + px4_sem_getvalue(&_sem, &val); + + while (val > 0) { + px4_sem_trywait(&_sem); + px4_sem_getvalue(&_sem, &val); + } + + while (px4_sem_wait(&_sem) != 0) {} + } + } + + hrt_cancel(&_hrt_call); +} + +void TaskWatchdog::capture_and_write_dump() +{ + mkdir(LOG_PATH_BASE, S_IRWXU | S_IRWXG | S_IRWXO); + + char path[64]; + int ret = format_file_path(LOG_WDG, path, sizeof(path)); + + if (ret != PX4_OK) { + PX4_ERR("Could not create watchdog dump file: %d", ret); + return; + } + + int fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666); + + if (fd < 0) { + PX4_ERR("failed to create %s: %d", path, errno); + return; + } + + dprintf(fd, "Task Watchdog Dump\n"); + dprintf(fd, "FW git-hash: %s\n", px4_firmware_version_string()); + dprintf(fd, "Build datetime: %s %s\n", __DATE__, __TIME__); + dprintf(fd, "Build url: %s \n", px4_build_uri()); + + const hrt_abstime now = hrt_absolute_time(); + + /* Loop 1: capture under sched_lock */ + sched_lock(); + + for (int i = 0; i < CONFIG_FS_PROCFS_MAX_TASKS; i++) { + const system_load_taskinfo_s &task = system_load.tasks[i]; + + if (!task.valid || !task.tcb || task.tcb->pid == 0) { + continue; + } + + task_dump_s &d = _dumps[i]; + d.pid = task.tcb->pid; + d.state = task.tcb->task_state; + +#if CONFIG_TASK_NAME_SIZE > 0 + strncpy(d.name, task.tcb->name, CONFIG_TASK_NAME_SIZE); + d.name[CONFIG_TASK_NAME_SIZE] = '\0'; +#endif + + if (task.tcb->xcp.regs && task.tcb->task_state != TSTATE_TASK_RUNNING) { + memcpy(d.regs, task.tcb->xcp.regs, sizeof(d.regs)); + d.has_regs = true; + + const uintptr_t sp = d.regs[REG_R13]; + const uintptr_t stack_top = (uintptr_t)task.tcb->stack_base_ptr + + task.tcb->adj_stack_size; + + if (sp >= (uintptr_t)task.tcb->stack_base_ptr && sp < stack_top) { + unsigned words_avail = (stack_top - sp) / sizeof(uint32_t); + d.stack_words = (words_avail < STACK_DUMP_WORDS) ? words_avail : STACK_DUMP_WORDS; + memcpy(d.stack, (const void *)sp, d.stack_words * sizeof(uint32_t)); + } + } + + d.valid = true; + } + + sched_unlock(); + + /* Loop 2: write to file — no locks held */ + for (int i = 0; i < CONFIG_FS_PROCFS_MAX_TASKS; i++) { + const task_dump_s &d = _dumps[i]; + + if (!d.valid) { + continue; + } + + dprintf(fd, "T:%" PRIu64 " %s(%d) state:%d", + now, +#if CONFIG_TASK_NAME_SIZE > 0 + d.name, +#else + "?", +#endif + (int)d.pid, + (int)d.state); + + if (d.has_regs) { + dprintf(fd, "\n r0:%08" PRIx32 " r1:%08" PRIx32 " r2:%08" PRIx32 " r3:%08" PRIx32 + " r4:%08" PRIx32 " r5:%08" PRIx32 " r6:%08" PRIx32 " r7:%08" PRIx32 "\n", + d.regs[REG_R0], d.regs[REG_R1], + d.regs[REG_R2], d.regs[REG_R3], + d.regs[REG_R4], d.regs[REG_R5], + d.regs[REG_R6], d.regs[REG_R7]); + dprintf(fd, " r8:%08" PRIx32 " r9:%08" PRIx32 " r10:%08" PRIx32 " r11:%08" PRIx32 + " r12:%08" PRIx32 " sp:%08" PRIx32 " lr:%08" PRIx32 " pc:%08" PRIx32 "\n", + d.regs[REG_R8], d.regs[REG_R9], + d.regs[REG_R10], d.regs[REG_R11], + d.regs[REG_R12], d.regs[REG_R13], + d.regs[REG_R14], d.regs[REG_R15]); +#ifdef CONFIG_ARMV7M_USEBASEPRI + dprintf(fd, " xpsr:%08" PRIx32 " basepri:%08" PRIx32 "\n", + d.regs[REG_XPSR], d.regs[REG_BASEPRI]); +#else + dprintf(fd, " xpsr:%08" PRIx32 " primask:%08" PRIx32 "\n", + d.regs[REG_XPSR], d.regs[REG_PRIMASK]); +#endif + + if (d.stack_words > 0) { + dprintf(fd, "STK(%u@%08" PRIx32 "):", d.stack_words, d.regs[REG_R13]); + + for (unsigned j = 0; j < d.stack_words; j++) { + if (j > 0 && (j % 8) == 0) { + dprintf(fd, "\n"); + } + + dprintf(fd, " %08" PRIx32, d.stack[j]); + } + + dprintf(fd, "\n"); + } + + } else { + dprintf(fd, " [no saved regs]\n"); + } + } + + fsync(fd); + close(fd); + PX4_INFO("dump written to %s", path); +} + +void TaskWatchdog::write_cpuload_file() +{ + mkdir(LOG_PATH_BASE, S_IRWXU | S_IRWXG | S_IRWXO); + + char path[64]; + int ret = format_file_path(LOG_LOAD, path, sizeof(path)); + + if (ret != PX4_OK) { + PX4_ERR("Could not create load dump file: %d", ret); + return; + } + + int fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666); + + if (fd < 0) { + PX4_ERR("failed to create %s: %d", path, errno); + return; + } + + print_load(fd, &_load); + + fsync(fd); + close(fd); + PX4_INFO("cpuload written to %s", path); +} + +int TaskWatchdog::format_file_path(log_type_t type, char *buf, size_t bufsz) +{ + if (!buf || bufsz == 0) { + return -EINVAL; + } + + struct timespec ts; + + struct tm tm; + + char timebuf[32]; + + if (clock_gettime(CLOCK_REALTIME, &ts) != 0) { + return -errno; + } + + if (!localtime_r(&ts.tv_sec, &tm)) { + return -EINVAL; + } + + if (strftime(timebuf, sizeof(timebuf), TIME_FMT, &tm) == 0) { + return -EINVAL; + } + + const char *fmt = (type == LOG_WDG) ? LOG_WDG_NAME_FMT : LOG_LOAD_NAME_FMT; + + int n = snprintf(buf, bufsz, "%s/", LOG_PATH_BASE); + + if (n < 0 || (size_t)n >= bufsz) { + return -ENAMETOOLONG; + } + + int m = snprintf(buf + n, bufsz - n, fmt, timebuf); + + if (m < 0 || (size_t)m >= bufsz - n) { + return -ENAMETOOLONG; + } + + return PX4_OK; +} + +int TaskWatchdog::custom_command(int argc, char *argv[]) +{ + if (!is_running(desc)) { + print_usage("task_watchdog not running"); + return 1; + } + + if (!strcmp(argv[0], "trigger")) { + get_instance(desc)->_shared.manual_trigger = true; + PX4_INFO("manual watchdog trigger requested"); + return 0; + } + + return print_usage("unknown command"); +} + +int TaskWatchdog::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Detects when a higher-priority task starves the system by running too long. +When starvation is detected, dumps the offending task's registers and stack, +and saves a cpuload snapshot. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("task_watchdog", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("trigger", "Manually trigger the watchdog"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +} // namespace task_watchdog + +extern "C" __EXPORT int task_watchdog_main(int argc, char *argv[]) +{ + return ModuleBase::main(task_watchdog::TaskWatchdog::desc, argc, argv); +} diff --git a/src/modules/task_watchdog/TaskWatchdog.hpp b/src/modules/task_watchdog/TaskWatchdog.hpp new file mode 100644 index 0000000000..8a76145d83 --- /dev/null +++ b/src/modules/task_watchdog/TaskWatchdog.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define LOG_PATH_BASE CONFIG_BOARD_ROOT_PATH "/task_watchdog" +#define LOG_WDG_NAME_FMT "wdg_%s.log" +#define LOG_LOAD_NAME_FMT "load_%s.log" +#define TIME_FMT "%Y_%m_%d_%H_%M_%S" + +using namespace time_literals; + +namespace task_watchdog +{ + +static constexpr unsigned STACK_DUMP_WORDS = 16; + +struct task_dump_s { + pid_t pid{0}; + uint8_t state{0}; +#if CONFIG_TASK_NAME_SIZE > 0 + char name[CONFIG_TASK_NAME_SIZE + 1] {}; +#endif + uint32_t regs[XCPTCONTEXT_REGS] {}; + uint32_t stack[STACK_DUMP_WORDS] {}; + unsigned stack_words{0}; + bool has_regs{false}; + bool valid{false}; +}; + +/* Shared state between ISR and task */ +struct watchdog_shared_s { + int monitored_task_index{-1}; ///< system_load index of this module's task + hrt_abstime ready_to_run_timestamp{0}; + uint8_t last_state{TSTATE_TASK_INVALID}; + int original_priority{0}; + hrt_abstime trigger_time{0}; + px4::atomic_bool triggered{false}; + bool manual_trigger{false}; ///< set from custom_command, checked by ISR + px4_sem_t *sem{nullptr}; ///< posted by ISR to wake the task immediately +}; + +typedef enum { + LOG_WDG, + LOG_LOAD +} log_type_t; + +class TaskWatchdog : public ModuleBase +{ +public: + static Descriptor desc; + + TaskWatchdog(); + ~TaskWatchdog() override; + + static int task_spawn(int argc, char *argv[]); + static int run_trampoline(int argc, char *argv[]); + static int custom_command(int argc, char *argv[]); + static int print_usage(const char *reason = nullptr); + + void run() override; + +private: + void start(); + + /* Capture all tasks and write register+stack dump to a file. Called from task context at boosted priority. */ + void capture_and_write_dump(); + + /* Write cpuload snapshot to a file. Called from task context. */ + void write_cpuload_file(); + + int format_file_path(log_type_t type, char *buf, size_t bufsz); + + /* HRT ISR callback which monitors whether this task is being starved. */ + static void isr_callback(void *arg); + + hrt_call _hrt_call{}; + watchdog_shared_s _shared{}; + px4_sem_t _sem; + task_dump_s _dumps[CONFIG_FS_PROCFS_MAX_TASKS] {}; + print_load_s _load{}; + + bool _cpuload_pending{false}; + + static constexpr hrt_abstime TRIGGER_THRESHOLD = 2_s; ///< time in ready to run to trigger watchdog + static constexpr hrt_abstime CPULOAD_ACCUMULATE_TIME = 300_ms; ///< how long to accumulate cpuload data +}; + +} // namespace task_watchdog