From f88f224fe6d4f9c5c701a861b0a473d34f5db616 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 15 Feb 2021 11:19:00 +0100
Subject: [PATCH] logger: avoid data bursts by distributing slow subscription
 updates over time

There's an increasing amount of slow logged topics at 1-2Hz, which were all
updated in the same logger iteration, leading to data bursts. For log
streaming this started to exceed uart buffer sizes. By distributing updates
more equal over time those bursts are removed, reducing buffer size
requirements.

Tests showed during steady state a reduction of maximum topic updates per
iteration from 40 down to 17.
Also the SD log buffer fill level is more constant.
---
 .../common/uORB/SubscriptionInterval.hpp      |  5 +++
 src/modules/logger/logger.cpp                 | 32 +++++++++++++++++++
 src/modules/logger/logger.h                   |  2 ++
 3 files changed, 39 insertions(+)

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};