From 7140914d380c62d90c6e163285f9b2a1b985089d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 18 Oct 2016 10:08:18 +0200
Subject: [PATCH] logger: remove LoggerSubscription::time_tried_subscribe

Instead use a single timestamp for subscription checks. This frees up
~800B of RAM.

Also make sure the subscription checks are distributed over time. On each
update, at most 1 topic subscription is checked. Reduces the load of the
logger from 7.3% to 5.8% (Pixracer)
---
 src/modules/logger/logger.cpp | 29 ++++++++++++++++++++++-------
 src/modules/logger/logger.h   |  6 ++----
 2 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 349aa01d31..6635eb1ccb 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -464,16 +464,12 @@ int Logger::add_topic(const char *name, unsigned interval = 0)
 	return fd;
 }
 
-bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer)
+bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer, bool try_to_subscribe)
 {
 	bool updated = false;
 	int &handle = sub.fd[multi_instance];
 
-	// only try to subscribe to topic if this is the first time
-	// after that just check after a certain interval to avoid high cpu usage
-	if (handle < 0 && (sub.time_tried_subscribe == 0 ||
-			   hrt_elapsed_time(&sub.time_tried_subscribe) > TRY_SUBSCRIBE_INTERVAL)) {
-		sub.time_tried_subscribe = hrt_absolute_time();
+	if (handle < 0 && try_to_subscribe) {
 
 		if (OK == orb_exists(sub.metadata, multi_instance)) {
 			handle = orb_subscribe_multi(sub.metadata, multi_instance);
@@ -722,6 +718,9 @@ void Logger::run()
 	px4_sem_init(&timer_semaphore, 0, 0);
 	hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_semaphore);
 
+	// check for new subscription data
+	hrt_abstime next_subscribe_check = 0;
+	int next_subscribe_topic_index = -1; // this is used to distribute the checks over time
 
 	while (!_task_should_exit) {
 
@@ -800,6 +799,8 @@ void Logger::run()
 			/* wait for lock on log buffer */
 			_writer.lock();
 
+			int sub_idx = 0;
+
 			for (LoggerSubscription &sub : _subscriptions) {
 				/* each message consists of a header followed by an orb data object
 				 */
@@ -809,7 +810,8 @@ void Logger::run()
 				 * and write a message to the log
 				 */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
-					if (copy_if_updated_multi(sub, instance, _msg_buffer + sizeof(ulog_message_data_header_s))) {
+					if (copy_if_updated_multi(sub, instance, _msg_buffer + sizeof(ulog_message_data_header_s),
+								  sub_idx == next_subscribe_topic_index)) {
 
 						uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
 						//write one byte after another (necessary because of alignment)
@@ -835,6 +837,8 @@ void Logger::run()
 						}
 					}
 				}
+
+				++sub_idx;
 			}
 
 			//check for new logging message(s)
@@ -873,6 +877,17 @@ void Logger::run()
 				_writer.notify();
 			}
 
+			/* subscription update */
+			if (next_subscribe_topic_index != -1) {
+				if (++next_subscribe_topic_index >= _subscriptions.size()) {
+					next_subscribe_topic_index = -1;
+					next_subscribe_check = hrt_absolute_time() + TRY_SUBSCRIBE_INTERVAL;
+				}
+
+			} else if (hrt_absolute_time() > next_subscribe_check) {
+				next_subscribe_topic_index = 0;
+			}
+
 #ifdef DBGPRINT
 			double deltat = (double)(hrt_absolute_time() - timer_start)  * 1e-6;
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 019256d6d8..17cc8745d8 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -61,7 +61,6 @@ namespace logger
 struct LoggerSubscription {
 	int fd[ORB_MULTI_MAX_INSTANCES];
 	uint16_t msg_ids[ORB_MULTI_MAX_INSTANCES];
-	uint64_t time_tried_subscribe;	// captures the time at which we checked last time if this instance existed
 	const orb_metadata *metadata = nullptr;
 
 	LoggerSubscription() {}
@@ -70,7 +69,6 @@ struct LoggerSubscription {
 		metadata(metadata_)
 	{
 		fd[0] = fd_;
-		time_tried_subscribe = 0;
 
 		for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) {
 			fd[i] = -1;
@@ -189,7 +187,7 @@ private:
 
 	void write_changed_parameters();
 
-	bool copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer);
+	bool copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer, bool try_to_subscribe);
 
 	/**
 	 * Write exactly one ulog message to the logger and handle dropouts.
@@ -228,9 +226,9 @@ private:
 
 	uint8_t						*_msg_buffer = nullptr;
 	int						_msg_buffer_len = 0;
-	bool						_task_should_exit = true;
 	char 						_log_dir[LOG_DIR_LEN];
 	char 						_log_file_name[32];
+	bool						_task_should_exit = true;
 	bool						_has_log_dir = false;
 	bool						_was_armed = false;
 	bool						_arm_override;