diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 81e3909cd9..5a447c189f 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -895,7 +895,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener cpuload; top once; listener cpuload"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1 -a"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' @@ -967,7 +967,7 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "px4io status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "sensors status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "top once"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener cpuload; top once; listener cpuload"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1 -a"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"' diff --git a/src/lib/systemlib/printload.h b/platforms/common/include/px4_platform_common/printload.h similarity index 81% rename from src/lib/systemlib/printload.h rename to platforms/common/include/px4_platform_common/printload.h index 70498930c4..e72b13633a 100644 --- a/src/lib/systemlib/printload.h +++ b/platforms/common/include/px4_platform_common/printload.h @@ -50,22 +50,22 @@ #endif struct print_load_s { - uint64_t total_user_time; + uint64_t total_user_time{0}; - int running_count; - int blocked_count; + int running_count{0}; + int blocked_count{0}; - uint64_t new_time; - uint64_t interval_start_time; - uint32_t last_times[CONFIG_MAX_TASKS]; // in [ms]. This wraps if a process needs more than 49 days of CPU - float interval_time_ms_inv; + uint64_t new_time{0}; + uint64_t interval_start_time{0}; + uint64_t last_times[CONFIG_MAX_TASKS] {}; + float interval_time_us{0.f}; }; __BEGIN_DECLS -__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); +__EXPORT void init_print_load(struct print_load_s *s); -__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); +__EXPORT void print_load(int fd, struct print_load_s *print_state); typedef void (*print_load_callback_f)(void *user); @@ -73,7 +73,7 @@ typedef void (*print_load_callback_f)(void *user); /** * Print load to a buffer, and call cb after each written line (buffer will not include '\n') */ -void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_callback_f cb, void *user, +void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb, void *user, struct print_load_s *print_state); __END_DECLS diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index d60d949106..efa2fd6082 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -39,7 +39,9 @@ if(NOT PX4_BOARD MATCHES "px4_io") board_dma_alloc.c board_fat_dma_alloc.c console_buffer.cpp + cpuload.cpp gpio.c + print_load.cpp tasks.cpp px4_nuttx_impl.cpp px4_init.cpp diff --git a/src/lib/systemlib/cpuload.c b/platforms/nuttx/src/px4/common/cpuload.cpp similarity index 65% rename from src/lib/systemlib/cpuload.c rename to platforms/nuttx/src/px4/common/cpuload.cpp index 5247ae2978..67184bdf15 100644 --- a/src/lib/systemlib/cpuload.c +++ b/platforms/nuttx/src/px4/common/cpuload.cpp @@ -40,25 +40,14 @@ * @author Petri Tanskanen */ #include - -#include -#include -#include - -//#include - -//#include - -#include - +#include +#include #include -#include "cpuload.h" +#include -#ifdef CONFIG_SCHED_INSTRUMENTATION - -#ifdef __PX4_NUTTX +#if defined(__PX4_NUTTX) && defined(CONFIG_SCHED_INSTRUMENTATION) # include @@ -71,13 +60,37 @@ __EXPORT struct system_load_s system_load; extern FAR struct tcb_s *sched_gettcb(pid_t pid); +static px4::atomic_int cpuload_monitor_all_count{0}; + +void cpuload_monitor_start() +{ + if (cpuload_monitor_all_count.fetch_add(1) == 0) { + // if the count was previously 0 (idle thread only) then clear any existing runtime data + sched_lock(); + + system_load.start_time = hrt_absolute_time(); + + for (int i = 1; i < CONFIG_MAX_TASKS; i++) { + system_load.tasks[i].total_runtime = 0; + system_load.tasks[i].curr_start_time = 0; + } + + sched_unlock(); + } +} + +void cpuload_monitor_stop() +{ + if (cpuload_monitor_all_count.fetch_sub(1) <= 1) { + // don't allow the count to go negative + cpuload_monitor_all_count.store(0); + } +} + void cpuload_initialize_once() { - system_load.start_time = hrt_absolute_time(); - int i; - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - system_load.tasks[i].valid = false; + for (auto &task : system_load.tasks) { + task.valid = false; } int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init" @@ -106,17 +119,15 @@ void cpuload_initialize_once() void sched_note_start(FAR struct tcb_s *tcb) { - /* search first free slot */ - int i; - + // find first free slot if (system_load.initialized) { - for (i = 1; i < CONFIG_MAX_TASKS; i++) { - if (!system_load.tasks[i].valid) { - /* slot is available */ - system_load.tasks[i].total_runtime = 0; - system_load.tasks[i].curr_start_time = 0; - system_load.tasks[i].tcb = tcb; - system_load.tasks[i].valid = true; + for (auto &task : system_load.tasks) { + if (!task.valid) { + // slot is available + task.total_runtime = 0; + task.curr_start_time = 0; + task.tcb = tcb; + task.valid = true; system_load.total_count++; break; } @@ -126,16 +137,14 @@ void sched_note_start(FAR struct tcb_s *tcb) void sched_note_stop(FAR struct tcb_s *tcb) { - int i; - if (system_load.initialized) { - for (i = 1; i < CONFIG_MAX_TASKS; i++) { - if (system_load.tasks[i].tcb != 0 && system_load.tasks[i].tcb->pid == tcb->pid) { - /* mark slot as fee */ - system_load.tasks[i].valid = false; - system_load.tasks[i].total_runtime = 0; - system_load.tasks[i].curr_start_time = 0; - system_load.tasks[i].tcb = NULL; + for (auto &task : system_load.tasks) { + if (task.tcb && task.tcb->pid == tcb->pid) { + // mark slot as free + task.valid = false; + task.total_runtime = 0; + task.curr_start_time = 0; + task.tcb = nullptr; system_load.total_count--; break; } @@ -145,14 +154,23 @@ void sched_note_stop(FAR struct tcb_s *tcb) void sched_note_suspend(FAR struct tcb_s *tcb) { - if (system_load.initialized) { - uint64_t new_time = hrt_absolute_time(); + if (tcb->pid == 0) { + system_load.tasks[0].total_runtime += hrt_elapsed_time(&system_load.tasks[0].curr_start_time); + return; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) { - /* Task ending its current scheduling run */ - if (system_load.tasks[i].valid && system_load.tasks[i].tcb != 0 && system_load.tasks[i].tcb->pid == tcb->pid) { - system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; + } else { + if (cpuload_monitor_all_count.load() == 0) { + return; + } + } + + for (auto &task : system_load.tasks) { + // Task ending its current scheduling run + if (task.valid && (task.curr_start_time > 0) + && task.tcb && task.tcb->pid == tcb->pid) { + + task.total_runtime += hrt_elapsed_time(&task.curr_start_time); break; } } @@ -161,25 +179,26 @@ void sched_note_suspend(FAR struct tcb_s *tcb) void sched_note_resume(FAR struct tcb_s *tcb) { - if (system_load.initialized) { - uint64_t new_time = hrt_absolute_time(); + if (tcb->pid == 0) { + hrt_store_absolute_time(&system_load.tasks[0].curr_start_time); + return; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) { - if (system_load.tasks[i].valid && system_load.tasks[i].tcb->pid == tcb->pid) { + } else { + if (cpuload_monitor_all_count.load() == 0) { + return; + } + } + + for (auto &task : system_load.tasks) { + if (task.valid && task.tcb && task.tcb->pid == tcb->pid) { // curr_start_time is accessed from an IRQ handler (in logger), so we need // to make the update atomic - irqstate_t irq_state = px4_enter_critical_section(); - system_load.tasks[i].curr_start_time = new_time; - px4_leave_critical_section(irq_state); + hrt_store_absolute_time(&task.curr_start_time); break; } } } - } -#else -__EXPORT struct system_load_s system_load; -#endif -#endif /* CONFIG_SCHED_INSTRUMENTATION */ +#endif // PX4_NUTTX && CONFIG_SCHED_INSTRUMENTATION diff --git a/src/lib/systemlib/cpuload.h b/platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h similarity index 79% rename from src/lib/systemlib/cpuload.h rename to platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h index 5bad4b7a85..2fc8bb80a3 100644 --- a/src/lib/systemlib/cpuload.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h @@ -40,21 +40,18 @@ #include struct system_load_taskinfo_s { - uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load - uint64_t curr_start_time; ///< Start time of the current scheduling slot -#ifdef __PX4_NUTTX - FAR struct tcb_s *tcb; ///< -#endif - bool valid; ///< Task is currently active / valid + uint64_t total_runtime{0}; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load + uint64_t curr_start_time{0}; ///< Start time of the current scheduling slot + struct tcb_s *tcb {nullptr}; + bool valid{false}; ///< Task is currently active / valid }; struct system_load_s { - uint64_t start_time; ///< Global start time of measurements - struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; - uint8_t initialized; - int total_count; - int running_count; - int sleeping_count; + uint64_t start_time{0}; + system_load_taskinfo_s tasks[CONFIG_MAX_TASKS] {}; + int total_count{0}; + int running_count{0}; + bool initialized{false}; }; __BEGIN_DECLS @@ -63,6 +60,9 @@ __EXPORT extern struct system_load_s system_load; __EXPORT void cpuload_initialize_once(void); +__EXPORT void cpuload_monitor_start(void); +__EXPORT void cpuload_monitor_stop(void); + __END_DECLS #endif diff --git a/src/lib/systemlib/print_load_nuttx.c b/platforms/nuttx/src/px4/common/print_load.cpp similarity index 78% rename from src/lib/systemlib/print_load_nuttx.c rename to platforms/nuttx/src/px4/common/print_load.cpp index c7edbf1437..ecfeb1fed0 100644 --- a/src/lib/systemlib/print_load_nuttx.c +++ b/platforms/nuttx/src/px4/common/print_load.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2020 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 @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file printload.c + * @file print_load.cpp * * Print the current system load. * @@ -42,8 +42,8 @@ #include #include -#include -#include +#include +#include #include #if defined(BOARD_DMA_ALLOC_POOL_SIZE) @@ -64,26 +64,31 @@ extern struct system_load_s system_load; #define CL "\033[K" // clear line -void init_print_load_s(uint64_t t, struct print_load_s *s) +void init_print_load(struct print_load_s *s) { + cpuload_monitor_start(); s->total_user_time = 0; s->running_count = 0; s->blocked_count = 0; - s->new_time = t; - s->interval_start_time = t; + s->new_time = system_load.start_time; + s->interval_start_time = system_load.start_time; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + sched_lock(); + // special case for IDLE thread + s->last_times[0] = system_load.tasks[0].total_runtime; + sched_unlock(); + + for (int i = 1; i < CONFIG_MAX_TASKS; i++) { s->last_times[i] = 0; } - s->interval_time_ms_inv = 0.f; + s->interval_time_us = 0.f; } -static const char * -tstate_name(const tstate_t s) +static constexpr const char *tstate_name(const tstate_t s) { switch (s) { case TSTATE_TASK_INVALID: @@ -127,24 +132,31 @@ tstate_name(const tstate_t s) } } -void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_callback_f cb, void *user, +void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb, void *user, struct print_load_s *print_state) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wformat" // NuttX uses a different printf format #pragma GCC diagnostic ignored "-Wformat-extra-args" - print_state->new_time = t; - int i; - uint64_t curr_time_us; - uint64_t idle_time_us; float idle_load = 0.f; - curr_time_us = t; - idle_time_us = system_load.tasks[0].total_runtime; + // create a copy of the runtimes because this could be updated during the print output + uint64_t total_runtime[CONFIG_MAX_TASKS] {}; + sched_lock(); + + print_state->new_time = hrt_absolute_time(); + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid) { + total_runtime[i] = system_load.tasks[i].total_runtime; + } + } + + sched_unlock(); if (print_state->new_time > print_state->interval_start_time) { - print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000)); + print_state->interval_time_us = print_state->new_time - print_state->interval_start_time; /* header for task list */ snprintf(buffer, buffer_length, "%4s %-*s %8s %6s %11s %10s %-5s %2s", @@ -161,22 +173,16 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c #endif "FD" ); - cb(user); + cb(user); } print_state->running_count = 0; print_state->blocked_count = 0; print_state->total_user_time = 0; - // create a copy of the runtimes because this could be updated during the print output - uint32_t total_runtime[CONFIG_MAX_TASKS]; - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - total_runtime[i] = (uint32_t)(system_load.tasks[i].total_runtime / 1000); - } - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { sched_lock(); // need to lock the tcb access (but make it as short as possible) @@ -185,7 +191,6 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c continue; } - uint64_t interval_runtime; unsigned tcb_pid = system_load.tasks[i].tcb->pid; size_t stack_size = system_load.tasks[i].tcb->adj_stack_size; ssize_t stack_free = 0; @@ -213,8 +218,8 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c #if CONFIG_RR_INTERVAL > 0 unsigned tcb_timeslice = system_load.tasks[i].tcb->timeslice; #endif - unsigned tcb_task_state = system_load.tasks[i].tcb->task_state; - unsigned tcb_sched_priority = system_load.tasks[i].tcb->sched_priority; + tstate_t tcb_task_state = (tstate_t)system_load.tasks[i].tcb->task_state; + uint8_t tcb_sched_priority = system_load.tasks[i].tcb->sched_priority; unsigned int tcb_num_used_fds = 0; // number of used file descriptors #if CONFIG_NFILE_DESCRIPTORS > 0 @@ -255,39 +260,43 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c case TSTATE_WAIT_SEM: print_state->blocked_count++; break; + + case TSTATE_TASK_STOPPED: + // DO NOTHING + break; + + case NUM_TASK_STATES: + // DO NOTHING + break; } - interval_runtime = (print_state->last_times[i] > 0 && total_runtime[i] > print_state->last_times[i]) - ? (total_runtime[i] - print_state->last_times[i]) : 0; - - print_state->last_times[i] = total_runtime[i]; - float current_load = 0.f; - if (print_state->new_time > print_state->interval_start_time) { - current_load = interval_runtime * print_state->interval_time_ms_inv; + if (total_runtime[i] > print_state->last_times[i]) { + const uint64_t interval_runtime = total_runtime[i] - print_state->last_times[i]; - if (tcb_pid != 0) { - print_state->total_user_time += interval_runtime; + current_load = interval_runtime / print_state->interval_time_us; + + if (tcb_pid == 0) { + idle_load = current_load; } else { - idle_load = current_load; + print_state->total_user_time += interval_runtime; } - } + print_state->last_times[i] = total_runtime[i]; if (print_state->new_time <= print_state->interval_start_time) { continue; // not enough data yet } // print output - int print_len = snprintf(buffer, buffer_length, "%4d %-*s %8d %2d.%03d %5u/%5u %3u (%3u) ", + int print_len = snprintf(buffer, buffer_length, "%4d %-*s %8d %6.3f %5u/%5u %3u (%3u) ", tcb_pid, CONFIG_TASK_NAME_SIZE, tcb_name, - total_runtime[i], - (int)(current_load * 100.0f), - (int)((current_load * 100.0f - (int)(current_load * 100.0f)) * 1000), + total_runtime[i] / 1000, // us -> ms + (double)(current_load * 100.f), stack_size - stack_free, stack_size, tcb_sched_priority, @@ -315,10 +324,8 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c // Print footer buffer[0] = 0; cb(user); - float task_load; - float sched_load; - task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv; + float task_load = (float)(print_state->total_user_time) / print_state->interval_time_us; /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ @@ -326,7 +333,7 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c task_load = (1.f - idle_load); } - sched_load = 1.f - idle_load - task_load; + const float sched_load = 1.f - idle_load - task_load; snprintf(buffer, buffer_length, "Processes: %d total, %d running, %d sleeping, max FDs: %d", system_load.total_count, @@ -354,8 +361,8 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c #endif snprintf(buffer, buffer_length, "Uptime: %.3fs total, %.3fs idle", - (double)curr_time_us / 1000000.d, - (double)idle_time_us / 1000000.d); + (double)print_state->new_time / 1e6, (double)total_runtime[0] / 1e6); + cb(user); print_state->interval_start_time = print_state->new_time; @@ -371,28 +378,28 @@ struct print_load_callback_data_s { static void print_load_callback(void *user) { - char *clear_line = ""; + char clear_line[] {CL}; struct print_load_callback_data_s *data = (struct print_load_callback_data_s *)user; - if (data->fd == 1) { - clear_line = CL; + if (data->fd != STDOUT_FILENO) { + clear_line[0] = '\0'; } dprintf(data->fd, "%s%s\n", clear_line, data->buffer); } -void print_load(uint64_t t, int fd, struct print_load_s *print_state) +void print_load(int fd, struct print_load_s *print_state) { - /* print system information */ - if (fd == 1) { - dprintf(fd, "\033[H"); /* move cursor home and clear screen */ + // print system information + if (fd == STDOUT_FILENO) { + // move cursor home and clear screen + dprintf(fd, "\033[H"); } - struct print_load_callback_data_s data; - + print_load_callback_data_s data{}; data.fd = fd; - print_load_buffer(t, data.buffer, sizeof(data.buffer), print_load_callback, &data, print_state); + print_load_buffer(data.buffer, sizeof(data.buffer), print_load_callback, &data, print_state); } #endif // if CONFIG_SCHED_INSTRUMENTATION diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 938c7cfb2a..30e6c46974 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include diff --git a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c index 34d22ff3a0..c5864f17e0 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c @@ -604,16 +604,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then) /** * Store the absolute time in an interrupt-safe fashion */ -hrt_abstime -hrt_store_absolute_time(volatile hrt_abstime *now) +void +hrt_store_absolute_time(volatile hrt_abstime *t) { irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime ts = hrt_absolute_time(); - + *t = hrt_absolute_time(); px4_leave_critical_section(flags); - - return ts; } /** diff --git a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c index 1558d9b8d3..7825271d4b 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c @@ -616,16 +616,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then) /** * Store the absolute time in an interrupt-safe fashion */ -hrt_abstime -hrt_store_absolute_time(volatile hrt_abstime *now) +void +hrt_store_absolute_time(volatile hrt_abstime *t) { irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime ts = hrt_absolute_time(); - + *t = hrt_absolute_time(); px4_leave_critical_section(flags); - - return ts; } /** diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c index fa7185842c..604020c525 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c @@ -604,16 +604,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then) /** * Store the absolute time in an interrupt-safe fashion */ -hrt_abstime -hrt_store_absolute_time(volatile hrt_abstime *now) +void +hrt_store_absolute_time(volatile hrt_abstime *t) { irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime ts = hrt_absolute_time(); - + *t = hrt_absolute_time(); px4_leave_critical_section(flags); - - return ts; } /** diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c index 5c78b6002f..a88a973f69 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -737,16 +737,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then) /** * Store the absolute time in an interrupt-safe fashion */ -hrt_abstime -hrt_store_absolute_time(volatile hrt_abstime *now) +void +hrt_store_absolute_time(volatile hrt_abstime *t) { irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime ts = hrt_absolute_time(); - + *t = hrt_absolute_time(); px4_leave_critical_section(flags); - - return ts; } /** diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 07457652f3..48656a2420 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -55,6 +55,8 @@ add_library(px4_layer px4_init.cpp lib_crc32.c drv_hrt.cpp + cpuload.cpp + print_load.cpp ${SHMEM_SRCS} ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") diff --git a/platforms/posix/src/px4/common/cpuload.cpp b/platforms/posix/src/px4/common/cpuload.cpp new file mode 100644 index 0000000000..542a66e01a --- /dev/null +++ b/platforms/posix/src/px4/common/cpuload.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +/** + * @file cpuload.cpp + * + * Measurement of CPU load of each individual task. + * + * @author Lorenz Meier + * @author Petri Tanskanen + */ +#include +#include +#include + +#include + +#include + +static px4::atomic_int cpuload_monitor_all_count{0}; + +void cpuload_monitor_start() +{ + cpuload_monitor_all_count.fetch_add(1); +} + +void cpuload_monitor_stop() +{ + if (cpuload_monitor_all_count.fetch_sub(1) <= 1) { + // don't all the count to go negative + cpuload_monitor_all_count.store(0); + } +} + +// TODO diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index c9c457b00a..fa93293389 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -176,13 +176,11 @@ hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ -hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) +void hrt_store_absolute_time(volatile hrt_abstime *t) { - hrt_abstime ts = hrt_absolute_time(); - return ts; + *t = hrt_absolute_time(); } - /* * If this returns true, the entry has been invoked and removed from the callout list, * or it has never been entered. diff --git a/platforms/posix/src/px4/common/include/px4_platform/cpuload.h b/platforms/posix/src/px4/common/include/px4_platform/cpuload.h new file mode 100644 index 0000000000..0674723184 --- /dev/null +++ b/platforms/posix/src/px4/common/include/px4_platform/cpuload.h @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + +__BEGIN_DECLS + +__EXPORT void cpuload_monitor_start(void); +__EXPORT void cpuload_monitor_stop(void); + +__END_DECLS diff --git a/src/lib/systemlib/print_load_posix.c b/platforms/posix/src/px4/common/print_load.cpp similarity index 90% rename from src/lib/systemlib/print_load_posix.c rename to platforms/posix/src/px4/common/print_load.cpp index 55536d3780..d385df4fda 100644 --- a/src/lib/systemlib/print_load_posix.c +++ b/platforms/posix/src/px4/common/print_load.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2020 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 @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file print_load_posix.c + * @file print_load.cpp * * Print the current system load. * @@ -46,8 +46,7 @@ #include #include -#include -#include +#include #include #ifdef __PX4_DARWIN @@ -63,32 +62,33 @@ extern struct system_load_s system_load; #define CL "\033[K" // clear line -void init_print_load_s(uint64_t t, struct print_load_s *s) +void init_print_load(struct print_load_s *s) { - s->total_user_time = 0; s->running_count = 0; s->blocked_count = 0; - s->new_time = t; - s->interval_start_time = t; + s->new_time = hrt_absolute_time(); + s->interval_start_time = s->new_time; for (int i = 0; i < CONFIG_MAX_TASKS; i++) { s->last_times[i] = 0; } - s->interval_time_ms_inv = 0.f; + s->interval_time_us = 0.f; } -void print_load(uint64_t t, int fd, struct print_load_s *print_state) +void print_load(int fd, struct print_load_s *print_state) { - char *clear_line = ""; + char clear_line[] = CL; /* print system information */ if (fd == 1) { dprintf(fd, "\033[H"); /* move cursor home and clear screen */ - clear_line = CL; + + } else { + memset(clear_line, 0, sizeof(clear_line)); } #if defined(__PX4_LINUX) || defined(__PX4_CYGWIN) || defined(__PX4_QURT) @@ -175,9 +175,8 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state) #endif } -void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_callback_f cb, void *user, +void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb, void *user, struct print_load_s *print_state) { } - diff --git a/platforms/qurt/src/px4/common/drv_hrt.cpp b/platforms/qurt/src/px4/common/drv_hrt.cpp index c4cee64797..0abb553d6c 100644 --- a/platforms/qurt/src/px4/common/drv_hrt.cpp +++ b/platforms/qurt/src/px4/common/drv_hrt.cpp @@ -156,13 +156,11 @@ hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ -hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) +void hrt_store_absolute_time(volatile hrt_abstime *t) { - hrt_abstime ts = hrt_absolute_time(); - return ts; + *t = hrt_absolute_time(); } - /* * If this returns true, the entry has been invoked and removed from the callout list, * or it has never been entered. diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 60fccff0e6..1bfe6c7127 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -124,7 +124,7 @@ __EXPORT extern hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime * * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ -__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); +__EXPORT extern void hrt_store_absolute_time(volatile hrt_abstime *time); #ifdef __PX4_QURT /** diff --git a/src/lib/systemlib/CMakeLists.txt b/src/lib/systemlib/CMakeLists.txt index 63255b25f5..155a1a2b24 100644 --- a/src/lib/systemlib/CMakeLists.txt +++ b/src/lib/systemlib/CMakeLists.txt @@ -42,15 +42,8 @@ set(SRCS if(${PX4_PLATFORM} STREQUAL "nuttx") list(APPEND SRCS - cpuload.c - cpuload.h - print_load_nuttx.c otp.c ) -else() - list(APPEND SRCS - print_load_posix.c - ) endif() px4_add_library(systemlib ${SRCS}) diff --git a/src/modules/load_mon/LoadMon.hpp b/src/modules/load_mon/LoadMon.hpp index b37215b8b2..f93ae94cfa 100644 --- a/src/modules/load_mon/LoadMon.hpp +++ b/src/modules/load_mon/LoadMon.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index afe3b7b923..1735bdd0a5 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -53,6 +53,7 @@ #include #include +#include #include #include #include @@ -1225,6 +1226,11 @@ void Logger::start_log_file(LogType type) return; } + if (type == LogType::Full) { + // initialize cpu load as early as possible to get more data + initialize_load_output(PrintLoadReason::Preflight); + } + PX4_INFO("Start file log (type: %s)", log_type_str(type)); char file_name[LOG_DIR_LEN] = ""; @@ -1260,8 +1266,6 @@ void Logger::start_log_file(LogType type) if (type == LogType::Full) { /* reset performance counters to get in-flight min and max values in post flight log */ perf_reset_all(); - - initialize_load_output(PrintLoadReason::Preflight); } _statistics[(int)type].start_time_file = hrt_absolute_time(); @@ -1289,6 +1293,9 @@ void Logger::start_log_mavlink() return; } + // initialize cpu load as early as possible to get more data + initialize_load_output(PrintLoadReason::Preflight); + PX4_INFO("Start mavlink log"); _writer.start_log_mavlink(); @@ -1304,8 +1311,6 @@ void Logger::start_log_mavlink() _writer.set_need_reliable_transfer(false); _writer.unselect_write_backend(); _writer.notify(); - - initialize_load_output(PrintLoadReason::Preflight); } void Logger::stop_log_mavlink() @@ -1388,16 +1393,8 @@ void Logger::print_load_callback(void *user) void Logger::initialize_load_output(PrintLoadReason reason) { - perf_callback_data_t callback_data; - callback_data.logger = this; - callback_data.counter = 0; - callback_data.buffer = nullptr; - char buffer[140]; - hrt_abstime curr_time = hrt_absolute_time(); - init_print_load_s(curr_time, &_load); - // this will not yet print anything - print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load); - _next_load_print = curr_time + 1000000; + init_print_load(&_load); + _next_load_print = hrt_absolute_time() + 1_s; _print_load_reason = reason; } @@ -1412,11 +1409,11 @@ void Logger::write_load_output() callback_data.logger = this; callback_data.counter = 0; callback_data.buffer = buffer; - hrt_abstime curr_time = hrt_absolute_time(); _writer.set_need_reliable_transfer(true); // TODO: maybe we should restrict the output to a selected backend (eg. when file logging is running // and mavlink log is started, this will be added to the file as well) - print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load); + print_load_buffer(buffer, sizeof(buffer), print_load_callback, &callback_data, &_load); + cpuload_monitor_stop(); _writer.set_need_reliable_transfer(false); } diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 2ee4a059ab..120be110c3 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/modules/logger/watchdog.h b/src/modules/logger/watchdog.h index 29def3d6bd..174d4eb441 100644 --- a/src/modules/logger/watchdog.h +++ b/src/modules/logger/watchdog.h @@ -37,7 +37,7 @@ #ifdef __PX4_NUTTX #include -#include +#include #endif /* __PX4_NUTTX */ namespace px4 diff --git a/src/systemcmds/top/CMakeLists.txt b/src/systemcmds/top/CMakeLists.txt index 781e561f06..351ef8a9e6 100644 --- a/src/systemcmds/top/CMakeLists.txt +++ b/src/systemcmds/top/CMakeLists.txt @@ -33,10 +33,11 @@ px4_add_module( MODULE systemcmds__top MAIN top + STACK_MAIN 4096 PRIORITY "SCHED_PRIORITY_MAX - 16" # max priority below high priority WQ threads SRCS - top.c + top.cpp DEPENDS systemlib ) diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.cpp similarity index 87% rename from src/systemcmds/top/top.c rename to src/systemcmds/top/top.cpp index 66c0ba1c05..0dd921d05a 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.cpp @@ -47,17 +47,11 @@ #include #include -#include -#include +#include +#include #include #include -/** - * Start the top application. - */ -__EXPORT int top_main(int argc, char *argv[]); - - static void print_usage(void) { PRINT_MODULE_DESCRIPTION("Monitor running processes and their CPU, stack usage, priority and state"); @@ -66,32 +60,30 @@ static void print_usage(void) PRINT_MODULE_USAGE_COMMAND_DESCR("once", "print load only once"); } -int -top_main(int argc, char *argv[]) +extern "C" __EXPORT int top_main(int argc, char *argv[]) { - hrt_abstime curr_time = hrt_absolute_time(); - - struct print_load_s load; - init_print_load_s(curr_time, &load); + print_load_s load{}; + init_print_load(&load); + px4_usleep(200000); /* clear screen */ dprintf(1, "\033[2J\n"); if (argc > 1) { if (!strcmp(argv[1], "once")) { - print_load(curr_time, 1, &load); px4_sleep(1); - print_load(hrt_absolute_time(), 1, &load); + print_load(STDOUT_FILENO, &load); } else { print_usage(); } + cpuload_monitor_stop(); return 0; } for (;;) { - print_load(curr_time, 1, &load); + print_load(STDOUT_FILENO, &load); /* Sleep 200 ms waiting for user input five times ~ 1s */ for (int k = 0; k < 5; k++) { @@ -108,6 +100,7 @@ top_main(int argc, char *argv[]) ret = read(0, &c, 1); if (ret) { + cpuload_monitor_stop(); return 1; } @@ -116,6 +109,7 @@ top_main(int argc, char *argv[]) case 0x1b: // esc case 'c': case 'q': + cpuload_monitor_stop(); return 0; /* not reached */ } @@ -123,9 +117,8 @@ top_main(int argc, char *argv[]) px4_usleep(200000); } - - curr_time = hrt_absolute_time(); } + cpuload_monitor_stop(); return 0; }