diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 0f313b4b68..083c93a958 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -124,6 +124,7 @@ set(msg_file_names vehicle_roi.msg mount_orientation.msg collision_report.msg + low_stack.msg ) # Get absolute paths diff --git a/msg/low_stack.msg b/msg/low_stack.msg new file mode 100644 index 0000000000..e58ce832f8 --- /dev/null +++ b/msg/low_stack.msg @@ -0,0 +1,4 @@ +uint8 MAX_REPORT_TASK_NAME_LEN = 16 + +uint16 stack_free +uint8[16] task_name diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp index 88f9fc78bc..cafd7f16bd 100644 --- a/src/modules/load_mon/load_mon.cpp +++ b/src/modules/load_mon/load_mon.cpp @@ -36,6 +36,7 @@ * * @author Jonathan Challinger * @author Julian Oes */ #include @@ -55,6 +56,7 @@ #include #include +#include extern struct system_load_s system_load; @@ -96,6 +98,15 @@ private: /* Calculate the memory usage */ float _ram_used(); +#ifdef __PX4_NUTTX + /* Calculate stack usage */ + void _stack_usage(); + + struct low_stack_s _low_stack; + int _stack_task_index; + orb_advert_t _low_stack_pub; +#endif + bool _taskShouldExit; bool _taskIsRunning; struct work_s _work; @@ -105,8 +116,12 @@ private: hrt_abstime _last_idle_time; }; - LoadMon::LoadMon() : +#ifdef __PX4_NUTTX + _low_stack {}, + _stack_task_index(0), + _low_stack_pub(nullptr), +#endif _taskShouldExit(false), _taskIsRunning(false), _work{}, @@ -117,14 +132,14 @@ LoadMon::LoadMon() : LoadMon::~LoadMon() { - work_cancel(HPWORK, &_work); + work_cancel(LPWORK, &_work); _taskIsRunning = false; } int LoadMon::start() { /* Schedule a cycle to start things. */ - return work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, 0); + return work_queue(LPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, 0); } void LoadMon::stop() @@ -147,7 +162,7 @@ void LoadMon::_cycle() _compute(); if (!_taskShouldExit) { - work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, + work_queue(LPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, USEC2TICK(LOAD_MON_INTERVAL_US)); } } @@ -168,6 +183,10 @@ void LoadMon::_compute() _cpuload.load = 1.0f - (float)interval_idletime / (float)LOAD_MON_INTERVAL_US; _cpuload.ram_usage = _ram_used(); +#ifdef __PX4_NUTTX + _stack_usage(); +#endif + if (_cpuload_pub == nullptr) { _cpuload_pub = orb_advertise(ORB_ID(cpuload), &_cpuload); @@ -205,6 +224,49 @@ float LoadMon::_ram_used() #endif } +#ifdef __PX4_NUTTX +void LoadMon::_stack_usage() +{ + for (int i = _stack_task_index; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && system_load.tasks[i].tcb->pid > 0) { + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) { + break; + } + + stack_free++; + } + + /* + * Found task low on stack, report and exit. Continue here in next cycle. + */ + if (stack_free < 300) { + strncpy((char *)_low_stack.task_name, system_load.tasks[i].tcb->name, low_stack_s::MAX_REPORT_TASK_NAME_LEN); + _low_stack.stack_free = stack_free; + + if (_low_stack_pub == nullptr) { + _low_stack_pub = orb_advertise(ORB_ID(low_stack), &_low_stack); + + } else { + orb_publish(ORB_ID(low_stack), _low_stack_pub, &_low_stack); + } + + // continue at next index + _stack_task_index = i + 1; + return; + } + } + } + + _stack_task_index = 0; +} +#endif + /** * Print the correct usage. */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a84d604436..c99d2c67fa 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -111,6 +111,7 @@ #include #include #include +#include #include #include @@ -1222,6 +1223,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_land_detected_s land_detected; struct cpuload_s cpuload; struct vehicle_gps_position_s dual_gps_pos; + struct low_stack_s low_stack; } buf; memset(&buf, 0, sizeof(buf)); @@ -1284,6 +1286,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_RPL6_s log_RPL6; struct log_LOAD_s log_LOAD; struct log_DPRS_s log_DPRS; + struct log_STCK_s log_STCK; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1334,6 +1337,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int commander_state_sub; int cpuload_sub; int diff_pres_sub; + int low_stack_sub; } subs; subs.cmd_sub = -1; @@ -1377,6 +1381,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.commander_state_sub = -1; subs.cpuload_sub = -1; subs.diff_pres_sub = -1; + subs.low_stack_sub = -1; /* add new topics HERE */ @@ -2319,7 +2324,14 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_LOAD_MSG; log_msg.body.log_LOAD.cpu_load = buf.cpuload.load; LOGBUFFER_WRITE_AND_COUNT(LOAD); + } + /* --- STACK --- */ + if (copy_if_updated(ORB_ID(low_stack), &subs.low_stack_sub, &buf.low_stack)) { + log_msg.msg_type = LOG_STCK_MSG; + log_msg.body.log_STCK.stack_free = buf.low_stack.stack_free; + strncpy(log_msg.body.log_STCK.task_name, (char*)buf.low_stack.task_name, sizeof(log_msg.body.log_STCK.task_name)); + LOGBUFFER_WRITE_AND_COUNT(STCK); } pthread_mutex_lock(&logbuffer_mutex); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 6c66955b0f..e3ea3291f8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -634,6 +634,13 @@ struct log_DPRS_s { float temperature; }; +/* --- LOW STACK --- */ +#define LOG_STCK_MSG 63 +struct log_STCK_s { + char task_name[16]; + uint16_t stack_free; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -724,6 +731,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LAND, "B", "Landed"), LOG_FORMAT(LOAD, "f", "CPU"), LOG_FORMAT(DPRS, "Qffff", "errors,DPRESraw,DPRES,DPRESmax,Temp"), + LOG_FORMAT(STCK, "NH", "Task,Free"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"),