From deaf125c81cda9bc4a891759f39f40f4c56b1b42 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 16 Oct 2017 14:54:37 +0200
Subject: [PATCH] logger: check for newly published topics while not logging

If logger is started very early, orb_exists() will fail for a lot of
topics, they will be advertised within the next few seconds.
Logger already dynamically adds subscriptions during logging, but if we
do that before as well, we'll avoid any delays and having to subscribe
to a lot of topics all at once.
---
 src/modules/logger/logger.cpp | 24 ++++++++++++++++++++++--
 1 file changed, 22 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index ac108217f9..1ee8b7501c 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -971,9 +971,10 @@ void Logger::run()
 		}
 
 
-		if (_writer.is_started()) {
 
-			hrt_abstime loop_time = hrt_absolute_time();
+		const hrt_abstime loop_time = hrt_absolute_time();
+
+		if (_writer.is_started()) {
 
 			/* check if we need to output the process load */
 			if (_next_load_print != 0 && loop_time >= _next_load_print) {
@@ -1108,6 +1109,25 @@ void Logger::run()
 
 #endif /* DBGPRINT */
 
+		} else { // not logging
+
+			// try to subscribe to new topics, even if we don't log, so that:
+			// - we avoid subscribing to many topics at once, when logging starts
+			// - we'll get the data immediately once we start logging (no need to wait for the next subscribe timeout)
+			if (next_subscribe_topic_index != -1) {
+				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
+					if (_subscriptions[next_subscribe_topic_index].fd[instance] < 0) {
+						try_to_subscribe_topic(_subscriptions[next_subscribe_topic_index], instance);
+					}
+				}
+				if (++next_subscribe_topic_index >= _subscriptions.size()) {
+					next_subscribe_topic_index = -1;
+					next_subscribe_check = loop_time + TRY_SUBSCRIBE_INTERVAL;
+				}
+
+			} else if (loop_time > next_subscribe_check) {
+				next_subscribe_topic_index = 0;
+			}
 		}
 
 		// wait for next loop iteration...