diff --git a/platforms/common/include/px4_platform_common/tasks.h b/platforms/common/include/px4_platform_common/tasks.h index f8d1758c65..89468e36bc 100644 --- a/platforms/common/include/px4_platform_common/tasks.h +++ b/platforms/common/include/px4_platform_common/tasks.h @@ -141,8 +141,12 @@ typedef struct { // The navigation system needs to execute regularly but has no realtime needs #define SCHED_PRIORITY_NAVIGATION (SCHED_PRIORITY_DEFAULT + 5) // SCHED_PRIORITY_DEFAULT -#define SCHED_PRIORITY_LOG_WRITER (SCHED_PRIORITY_DEFAULT - 10) + #define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) + +#define SCHED_PRIORITY_LOG_WRITER (SCHED_PRIORITY_DEFAULT - 40) + + // SCHED_PRIORITY_IDLE typedef int (*px4_main_t)(int argc, char *argv[]); diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp index f9f08fddb6..17f1677dab 100644 --- a/src/modules/logger/log_writer_file.cpp +++ b/src/modules/logger/log_writer_file.cpp @@ -301,7 +301,7 @@ int LogWriterFile::thread_start() sched_param param; /* low priority, as this is expensive disk I/O */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; + param.sched_priority = SCHED_PRIORITY_LOG_WRITER; (void)pthread_attr_setschedparam(&thr_attr, ¶m); pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1170)); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ddb2ce9bf3..9c062fda1c 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -36,7 +36,6 @@ #include "logged_topics.h" #include "logger.h" #include "messages.h" -#include "watchdog.h" #include #include @@ -83,47 +82,6 @@ using namespace px4::logger; using namespace time_literals; - -struct timer_callback_data_s { - px4_sem_t semaphore; - - watchdog_data_t watchdog_data; - volatile bool watchdog_triggered = false; -}; - -/* This is used to schedule work for the logger (periodic scan for updated topics) */ -static void timer_callback(void *arg) -{ - /* Note: we are in IRQ context here (on NuttX) */ - - timer_callback_data_s *data = (timer_callback_data_s *)arg; - - int semaphore_value = 0; - - px4_sem_getvalue(&data->semaphore, &semaphore_value); - - /* check the value of the semaphore: if the logger cannot keep up with running it's main loop as fast - * as the timer_callback here increases the semaphore count, the counter would increase unbounded, - * leading to an overflow at some point. This case we want to avoid here, so we check the current - * value against a (somewhat arbitrary) threshold, and avoid calling sem_post() if it's exceeded. - * (it's not a problem if the threshold is a bit too large, it just means the logger will do - * multiple iterations at once, the next time it's scheduled). - * As the watchdog also uses the counter we use a conservatively high value */ - bool semaphore_value_saturated = semaphore_value > 100; - - if (watchdog_update(data->watchdog_data, semaphore_value_saturated)) { - data->watchdog_triggered = true; - } - - if (semaphore_value_saturated) { - return; - } - - px4_sem_post(&data->semaphore); - -} - - int logger_main(int argc, char *argv[]) { // logger currently assumes little endian @@ -142,6 +100,35 @@ namespace px4 namespace logger { +void Logger::timer_callback(void *arg) +{ + // Note: we are in IRQ context here (on NuttX) + timer_callback_data_s *data = static_cast(arg); + + int semaphore_value = 0; + + px4_sem_getvalue(&data->semaphore, &semaphore_value); + + /* check the value of the semaphore: if the logger cannot keep up with running it's main loop as fast + * as the timer_callback here increases the semaphore count, the counter would increase unbounded, + * leading to an overflow at some point. This case we want to avoid here, so we check the current + * value against a (somewhat arbitrary) threshold, and avoid calling sem_post() if it's exceeded. + * (it's not a problem if the threshold is a bit too large, it just means the logger will do + * multiple iterations at once, the next time it's scheduled). + * As the watchdog also uses the counter we use a conservatively high value */ + bool semaphore_value_saturated = semaphore_value > 100; + + if (watchdog_update(data->watchdog_data, semaphore_value_saturated)) { + data->watchdog_triggered.store(true); + } + + if (semaphore_value_saturated) { + return; + } + + px4_sem_post(&data->semaphore); +} + constexpr const char *Logger::LOG_ROOT[(int)LogType::Count]; int Logger::custom_command(int argc, char *argv[]) @@ -161,6 +148,16 @@ int Logger::custom_command(int argc, char *argv[]) return 0; } + if (!strcmp(argv[0], "priority_boost")) { + get_instance()->set_priority_boost_request(true); + return 0; + } + + if (!strcmp(argv[0], "priority_restore")) { + get_instance()->set_priority_boost_request(false); + return 0; + } + return print_usage("unknown command"); } @@ -649,11 +646,10 @@ void Logger::run() } /* init the update timer */ - struct hrt_call timer_call {}; - timer_callback_data_s timer_callback_data; - px4_sem_init(&timer_callback_data.semaphore, 0, 0); + _timer_call = {}; + px4_sem_init(&_timer_callback_data.semaphore, 0, 0); /* timer_semaphore use case is a signal */ - px4_sem_setprotocol(&timer_callback_data.semaphore, SEM_PRIO_NONE); + px4_sem_setprotocol(&_timer_callback_data.semaphore, SEM_PRIO_NONE); int polling_topic_sub = -1; @@ -673,10 +669,10 @@ void Logger::run() // sched_note_start is already called from pthread_create and task_create, // which means we can expect to find the tasks in system_load.tasks, as required in watchdog_initialize - watchdog_initialize(pid_self, writer_thread, timer_callback_data.watchdog_data); + watchdog_initialize(pid_self, writer_thread, _timer_callback_data.watchdog_data); } - hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_callback_data); + hrt_call_every(&_timer_call, _log_interval, _log_interval, timer_callback, &_timer_callback_data); } // check for new subscription data @@ -703,9 +699,9 @@ void Logger::run() /* check for logging command from MAVLink (start/stop streaming) */ handle_vehicle_command_update(); - if (timer_callback_data.watchdog_triggered) { - timer_callback_data.watchdog_triggered = false; + if (_timer_callback_data.watchdog_triggered.load()) { initialize_load_output(PrintLoadReason::Watchdog); + _timer_callback_data.watchdog_triggered.store(false); } @@ -916,7 +912,7 @@ void Logger::run() * And on linux this is quite accurate as well, but under NuttX it is not accurate, * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms). */ - while (px4_sem_wait(&timer_callback_data.semaphore) != 0) {} + while (px4_sem_wait(&_timer_callback_data.semaphore) != 0) {} } } @@ -925,8 +921,8 @@ void Logger::run() stop_log_file(LogType::Full); stop_log_file(LogType::Mission); - hrt_cancel(&timer_call); - px4_sem_destroy(&timer_callback_data.semaphore); + hrt_cancel(&_timer_call); + px4_sem_destroy(&_timer_callback_data.semaphore); // stop the writer thread _writer.thread_stop(); @@ -1587,7 +1583,7 @@ void Logger::initialize_load_output(PrintLoadReason reason) void Logger::write_load_output() { if (_print_load_reason == PrintLoadReason::Watchdog) { - PX4_ERR("Writing watchdog data"); // this is just that we see it easily in the log + PX4_WARN("Writing watchdog data"); // this is just that we see it easily in the log } perf_callback_data_t callback_data = {}; @@ -2421,6 +2417,12 @@ $ logger on PRINT_MODULE_USAGE_PARAM_FLOAT('c', 1.0, 0.2, 2.0, "Log rate factor (higher is faster)", true); PRINT_MODULE_USAGE_COMMAND_DESCR("on", "start logging now, override arming (logger must be running)"); PRINT_MODULE_USAGE_COMMAND_DESCR("off", "stop logging now, override arming (logger must be running)"); + +#if defined(__PX4_NUTTX) + PRINT_MODULE_USAGE_COMMAND_DESCR("priority_boost", "boost logger and log writer to maximum priority (logger must be running)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("priority_restore", "restore logger and log writer priority to default (logger must be running)"); +#endif // __PX4_NUTTX + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index c397600ad2..9254d1a654 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -36,12 +36,14 @@ #include "log_writer.h" #include "logged_topics.h" #include "messages.h" +#include "watchdog.h" #include #include "util.h" #include #include #include #include +#include #include #include #include @@ -56,6 +58,8 @@ #include #include + + extern "C" __EXPORT int logger_main(int argc, char *argv[]); using namespace time_literals; @@ -131,6 +135,8 @@ public: void set_arm_override(bool override) { _manually_logging_override = override; } + void set_priority_boost_request(bool request_boost) { _timer_callback_data.watchdog_data.priority_boost_requested.store(request_boost); } + private: enum class PrintLoadReason { @@ -326,6 +332,18 @@ private: void adjust_subscription_updates(); + /* This is used to schedule work for the logger (periodic scan for updated topics) */ + static void timer_callback(void *arg); + + struct timer_callback_data_s { + px4_sem_t semaphore{}; + + watchdog_data_t watchdog_data{}; + px4::atomic_bool watchdog_triggered{false}; + } _timer_callback_data{}; + + struct hrt_call _timer_call {}; + uint8_t *_msg_buffer{nullptr}; int _msg_buffer_len{0}; @@ -333,6 +351,7 @@ private: bool _prev_state{false}; ///< previous state depending on logging mode (arming or aux1 state) bool _manually_logging_override{false}; + bool _request_priority_boost{false}; Statistics _statistics[(int)LogType::Count]; hrt_abstime _last_sync_time{0}; ///< last time a sync msg was sent diff --git a/src/modules/logger/watchdog.cpp b/src/modules/logger/watchdog.cpp index 7618dd366c..081f4a7b18 100644 --- a/src/modules/logger/watchdog.cpp +++ b/src/modules/logger/watchdog.cpp @@ -34,6 +34,7 @@ #include "watchdog.h" #include +#include #if defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_INSTRUMENTATION) # error watchdog support requires CONFIG_SCHED_INSTRUMENTATION @@ -54,9 +55,11 @@ bool watchdog_update(watchdog_data_t &watchdog_data, bool semaphore_value_satura if (system_load.initialized && watchdog_data.logger_main_task_index >= 0 && watchdog_data.logger_writer_task_index >= 0) { const hrt_abstime now = hrt_absolute_time(); - const system_load_taskinfo_s &log_writer_task = system_load.tasks[watchdog_data.logger_writer_task_index]; - if (log_writer_task.valid) { + const system_load_taskinfo_s &logger_main_task = system_load.tasks[watchdog_data.logger_main_task_index]; + const system_load_taskinfo_s &logger_writer_task = system_load.tasks[watchdog_data.logger_writer_task_index]; + + if (logger_writer_task.valid) { // Trigger the watchdog if the log writer task has been ready to run for a // minimum duration and it has not been scheduled during that time. // When the writer is waiting for an SD transfer, it is not in ready state, thus a long dropout @@ -68,12 +71,12 @@ bool watchdog_update(watchdog_data_t &watchdog_data, bool semaphore_value_satura // No need to lock the tcb access, since we are in IRQ context // update the timestamp if it has been scheduled recently - if (log_writer_task.curr_start_time > watchdog_data.ready_to_run_timestamp) { - watchdog_data.ready_to_run_timestamp = log_writer_task.curr_start_time; + if (logger_writer_task.curr_start_time > watchdog_data.ready_to_run_timestamp) { + watchdog_data.ready_to_run_timestamp = logger_writer_task.curr_start_time; } // update the timestamp if not ready to run or if transitioned into ready to run - uint8_t current_state = log_writer_task.tcb->task_state; + uint8_t current_state = logger_writer_task.tcb->task_state; if (current_state != TSTATE_TASK_READYTORUN || (watchdog_data.last_state != TSTATE_TASK_READYTORUN && current_state == TSTATE_TASK_READYTORUN)) { @@ -106,23 +109,52 @@ bool watchdog_update(watchdog_data_t &watchdog_data, bool semaphore_value_satura watchdog_data.sem_counter_saturated_start = now; } - if (now - watchdog_data.sem_counter_saturated_start > 3_s || now - watchdog_data.ready_to_run_timestamp > 1_s) { + if (!watchdog_data.priority_boosted + && ((now - watchdog_data.sem_counter_saturated_start > 3_s) || (now - watchdog_data.ready_to_run_timestamp > 1_s) + || watchdog_data.priority_boost_requested.load()) + ) { // boost the priority to make sure the logger continues to write to the log. // Note that we never restore the priority, to keep the logic simple and because it is // an event that must not occur under normal circumstances (if it does, there's a bug // somewhere) - sched_param param{}; - param.sched_priority = SCHED_PRIORITY_MAX; - if (system_load.tasks[watchdog_data.logger_main_task_index].valid) { - sched_setparam(system_load.tasks[watchdog_data.logger_main_task_index].tcb->pid, ¶m); + if (logger_main_task.valid) { + sched_param param{}; + param.sched_priority = SCHED_PRIORITY_MAX; + sched_setparam(logger_main_task.tcb->pid, ¶m); } - sched_setparam(log_writer_task.tcb->pid, ¶m); + if (logger_writer_task.valid) { + sched_param param{}; + param.sched_priority = SCHED_PRIORITY_MAX; + sched_setparam(logger_writer_task.tcb->pid, ¶m); + } + + // reset to avoid trigger again immediately + watchdog_data.sem_counter_saturated_start = now; + watchdog_data.ready_to_run_timestamp = now; + + watchdog_data.priority_boosted = true; + watchdog_data.priority_boost_requested.store(true); - // make sure we won't trigger again - watchdog_data.logger_main_task_index = -1; return true; + + } else if (watchdog_data.priority_boosted && !watchdog_data.priority_boost_requested.load()) { + // restore original priorities + if (logger_main_task.valid) { + sched_param param{}; + param.sched_priority = SCHED_PRIORITY_LOG_CAPTURE; + sched_setparam(logger_main_task.tcb->pid, ¶m); + } + + if (logger_writer_task.valid) { + sched_param param{}; + param.sched_priority = SCHED_PRIORITY_LOG_WRITER; + sched_setparam(logger_writer_task.tcb->pid, ¶m); + } + + watchdog_data.priority_boosted = false; + watchdog_data.priority_boost_requested.store(false); } } else { diff --git a/src/modules/logger/watchdog.h b/src/modules/logger/watchdog.h index 1ba1843593..1ffad23002 100644 --- a/src/modules/logger/watchdog.h +++ b/src/modules/logger/watchdog.h @@ -34,6 +34,7 @@ #pragma once #include +#include #ifdef __PX4_NUTTX #include @@ -52,7 +53,10 @@ struct watchdog_data_t { hrt_abstime ready_to_run_timestamp = hrt_absolute_time(); hrt_abstime sem_counter_saturated_start = hrt_absolute_time(); uint8_t last_state = TSTATE_TASK_INVALID; + + bool priority_boosted{false}; #endif /* __PX4_NUTTX */ + px4::atomic_bool priority_boost_requested {false}; };