From a4206ba553d3693d7ad7e0ac224987ae790961a4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 28 Jan 2022 07:59:53 +0100
Subject: [PATCH] logger: log excluded optional topics

Can be displayed with:
ulog_info <file> -m excluded_optional_topics
---
 src/modules/logger/logged_topics.cpp |  5 +++++
 src/modules/logger/logged_topics.h   |  5 +++++
 src/modules/logger/logger.cpp        | 17 +++++++++++++++++
 src/modules/logger/logger.h          |  6 ++++++
 4 files changed, 33 insertions(+)

diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index a5e5570363..e7d6d8fb21 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -407,6 +407,11 @@ bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, ui
 
 	if (optional && orb_exists(topic, instance) != 0) {
 		PX4_DEBUG("Not adding non-existing optional topic %s %i", topic->o_name, instance);
+
+		if (instance == 0 && _subscriptions.num_excluded_optional_topic_ids < MAX_EXCLUDED_OPTIONAL_TOPICS_NUM) {
+			_subscriptions.excluded_optional_topic_ids[_subscriptions.num_excluded_optional_topic_ids++] = topic->o_id;
+		}
+
 		return false;
 	}
 
diff --git a/src/modules/logger/logged_topics.h b/src/modules/logger/logged_topics.h
index baa389e396..4a990d5ba7 100644
--- a/src/modules/logger/logged_topics.h
+++ b/src/modules/logger/logged_topics.h
@@ -77,6 +77,8 @@ class LoggedTopics
 public:
 	static constexpr int MAX_TOPICS_NUM = 255; /**< Maximum number of logged topics */
 
+	static constexpr int MAX_EXCLUDED_OPTIONAL_TOPICS_NUM = 40;
+
 	struct RequestedSubscription {
 		uint16_t interval_ms;
 		uint8_t instance;
@@ -85,6 +87,9 @@ public:
 	struct RequestedSubscriptionArray {
 		RequestedSubscription sub[MAX_TOPICS_NUM];
 		int count{0};
+
+		uint8_t excluded_optional_topic_ids[MAX_EXCLUDED_OPTIONAL_TOPICS_NUM];
+		int num_excluded_optional_topic_ids{0};
 	};
 
 	LoggedTopics() = default;
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 491f2a0baa..4e7fa75fe1 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -526,6 +526,10 @@ bool Logger::initialize_topics()
 		}
 	}
 
+	_num_excluded_optional_topic_ids = logged_topics.subscriptions().num_excluded_optional_topic_ids;
+	memcpy(_excluded_optional_topic_ids, logged_topics.subscriptions().excluded_optional_topic_ids,
+	       sizeof(_excluded_optional_topic_ids));
+
 	delete[](_subscriptions);
 	_subscriptions = nullptr;
 
@@ -1407,6 +1411,7 @@ void Logger::start_log_file(LogType type)
 		write_parameter_defaults(type);
 		write_perf_data(true);
 		write_console_output();
+		write_excluded_optional_topics(type);
 	}
 
 	write_all_add_logged_msg(type);
@@ -1463,6 +1468,7 @@ void Logger::start_log_mavlink()
 	write_parameter_defaults(LogType::Full);
 	write_perf_data(true);
 	write_console_output();
+	write_excluded_optional_topics(LogType::Full);
 	write_all_add_logged_msg(LogType::Full);
 	_writer.set_need_reliable_transfer(false);
 	_writer.unselect_write_backend();
@@ -1908,6 +1914,17 @@ void Logger::write_info_template(LogType type, const char *name, T value, const
 	_writer.unlock();
 }
 
+void Logger::write_excluded_optional_topics(LogType type)
+{
+	for (int i = 0; i < _num_excluded_optional_topic_ids; ++i) {
+		orb_id_t meta = get_orb_meta((ORB_ID)_excluded_optional_topic_ids[i]);
+
+		if (meta) {
+			write_info_multiple(type, "excluded_optional_topics", meta->o_name, false);
+		}
+	}
+}
+
 void Logger::write_header(LogType type)
 {
 	ulog_file_header_s header = {};
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index cb9249c530..2b42e02b2a 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -34,6 +34,7 @@
 #pragma once
 
 #include "log_writer.h"
+#include "logged_topics.h"
 #include "messages.h"
 #include <containers/Array.hpp>
 #include "util.h"
@@ -248,6 +249,8 @@ private:
 
 	void write_version(LogType type);
 
+	void write_excluded_optional_topics(LogType type);
+
 	void write_info(LogType type, const char *name, const char *value);
 	void write_info_multiple(LogType type, const char *name, const char *value, bool is_continued);
 	void write_info(LogType type, const char *name, int32_t value);
@@ -342,6 +345,9 @@ private:
 	uint16_t 					_event_sequence_offset{0}; ///< event sequence offset to account for skipped (not logged) messages
 	uint16_t 					_event_sequence_offset_mission{0};
 
+	uint8_t						_excluded_optional_topic_ids[LoggedTopics::MAX_EXCLUDED_OPTIONAL_TOPICS_NUM];
+	int						_num_excluded_optional_topic_ids{0};
+
 	LogWriter					_writer;
 	uint32_t					_log_interval{0};
 	float						_rate_factor{1.0f};