diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp index 36e629f26d..31d1b0a7af 100644 --- a/platforms/common/uORB/SubscriptionInterval.hpp +++ b/platforms/common/uORB/SubscriptionInterval.hpp @@ -152,6 +152,11 @@ public: */ void set_interval_ms(uint32_t interval) { _interval_us = interval * 1000; } + /** + * Set the last data update + * @param t should be in range [now, now - _interval_us] + */ + void set_last_update(hrt_abstime t) { _last_update = t; } protected: Subscription _subscription; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 8ba403e41d..a745b69f27 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -665,6 +665,8 @@ void Logger::run() _lockstep_component = px4_lockstep_register_component(); } + bool was_started = false; + while (!should_exit()) { // Start/stop logging (depending on logging mode, by default when arming/disarming) const bool logging_started = start_stop_logging(); @@ -689,6 +691,10 @@ void Logger::run() if (_writer.is_started(LogType::Full)) { // mission log only runs when full log is also started + if (!was_started) { + adjust_subscription_updates(); + } + /* check if we need to output the process load */ if (_next_load_print != 0 && loop_time >= _next_load_print) { _next_load_print = 0; @@ -835,6 +841,8 @@ void Logger::run() debug_print_buffer(total_bytes, timer_start); + was_started = true; + } else { // not logging // try to subscribe to new topics, even if we don't log, so that: @@ -853,6 +861,8 @@ void Logger::run() } else if (loop_time > next_subscribe_check) { next_subscribe_topic_index = 0; } + + was_started = false; } update_params(); @@ -1025,6 +1035,26 @@ void Logger::publish_logger_status() } } +void Logger::adjust_subscription_updates() +{ + // we want subscriptions to update evenly distributed over time to avoid + // data bursts. This is particularly important for low-rate topics + hrt_abstime now = hrt_absolute_time(); + int j = 0; + + for (int i = 0; i < _num_subscriptions; ++i) { + if (_subscriptions[i].get_interval_us() >= 500_ms) { + hrt_abstime adjustment = (_log_interval * j) % 500_ms; + + if (adjustment < now) { + _subscriptions[i].set_last_update(now - adjustment); + } + + ++j; + } + } +} + bool Logger::get_disable_boot_logging() { if (_param_sdlog_boot_bat.get()) { @@ -1386,6 +1416,8 @@ void Logger::start_log_mavlink() _writer.set_need_reliable_transfer(false); _writer.unselect_write_backend(); _writer.notify(); + + adjust_subscription_updates(); // redistribute updates as sending the header can take some time } void Logger::stop_log_mavlink() diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 7bf990b828..e2aaf8d5a7 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -318,6 +318,8 @@ private: */ bool handle_event_updates(uint32_t &total_bytes); + void adjust_subscription_updates(); + uint8_t *_msg_buffer{nullptr}; int _msg_buffer_len{0};