From 784cec1caeb045682f279bd4bc00862b2a9bca9e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 30 Jun 2017 11:41:30 +0200
Subject: [PATCH] logger: check if topic already added & if so only update the
 interval

---
 src/modules/logger/logger.cpp | 27 ++++++++++++++++++++-------
 1 file changed, 20 insertions(+), 7 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 2797272f4d..f89529f061 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -496,17 +496,30 @@ int Logger::add_topic(const char *name, unsigned interval = 0)
 
 	for (size_t i = 0; i < orb_topics_count(); i++) {
 		if (strcmp(name, topics[i]->o_name) == 0) {
-			fd = add_topic(topics[i]);
-			PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval);
-			break;
+			bool already_added = false;
+
+			// check if already added: if so, only update the interval
+			for (size_t j = 0; j < _subscriptions.size(); ++j) {
+				if (_subscriptions[j].metadata == topics[i]) {
+					PX4_DEBUG("logging topic %s, interval: %i, already added, only setting interval",
+						  topics[i]->o_name, interval);
+					fd = _subscriptions[j].fd[0];
+					already_added = true;
+					break;
+				}
+			}
+
+			if (!already_added) {
+				fd = add_topic(topics[i]);
+				PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval);
+				break;
+			}
 		}
 	}
 
 	// if we poll on a topic, we don't set the interval and let the polled topic define the maximum interval
-	if (!_polling_topic_meta) {
-		if (fd >= 0 && interval != 0) {
-			orb_set_interval(fd, interval);
-		}
+	if (!_polling_topic_meta && fd >= 0) {
+		orb_set_interval(fd, interval);
 	}
 
 	return fd;