logger: parallel mavlink logging

tiiuae · Oct 26, 2023 · 7f41dc1 · 7f41dc1
1 parent bdd8196
commit 7f41dc1
Show file tree

Hide file tree

Showing 9 changed files with 603 additions and 30 deletions.
diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt
@@ -30,6 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+if(CONFIG_LOGGER_PARALLEL_LOGGING)
+       set(LOGGER_PARALLEL_COMPILE_FLAG "-DLOGGER_PARALLEL_LOGGING")
+endif()
 
 px4_add_module(
 	MODULE modules__logger
@@ -38,6 +41,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
 		-Wno-cast-align # TODO: fix and enable
+		${LOGGER_PARALLEL_COMPILE_FLAG}
 	SRCS
 		logged_topics.cpp
 		logger.cpp

diff --git a/src/modules/logger/Kconfig b/src/modules/logger/Kconfig
@@ -10,3 +10,10 @@ menuconfig USER_LOGGER
 	depends on BOARD_PROTECTED && MODULES_LOGGER
 	---help---
 		Put logger in userspace memory
+
+menuconfig LOGGER_PARALLEL_LOGGING
+	bool "Custom mavlink logging protocol in logger"
+	default n
+	depends on MODULES_LOGGER
+	---help---
+		Utilize custom mavlink logging protocol for speed up logging start phase
diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
@@ -158,7 +158,7 @@ void LogWriter::thread_stop()
 	}
 }
 
-int LogWriter::write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start)
+int LogWriter::write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start, bool reliable, bool wait)
 {
 	int ret_file = 0, ret_mavlink = 0;
 
@@ -167,7 +167,16 @@ int LogWriter::write_message(LogType type, void *ptr, size_t size, uint64_t drop
 	}
 
 	if (_log_writer_mavlink_for_write && type == LogType::Full) {
-		ret_mavlink = _log_writer_mavlink_for_write->write_message(ptr, size);
+#ifdef LOGGER_PARALLEL_LOGGING
+
+		if (reliable) {
+			ret_mavlink = _log_writer_mavlink_for_write->write_reliable_message(ptr, size, wait);
+
+		} else
+#endif
+		{
+			ret_mavlink = _log_writer_mavlink_for_write->write_message(ptr, size);
+		}
 	}
 
 	// file backend errors takes precedence

diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
@@ -73,6 +73,19 @@ class LogWriter
 
 	void stop_log_mavlink();
 
+#ifdef LOGGER_PARALLEL_LOGGING
+	void wait_fifo_empty()
+	{
+		if (_log_writer_mavlink) {
+			_log_writer_mavlink->wait_fifo_count(0);
+
+			while (_log_writer_mavlink->reliable_fifo_is_sending()) {
+				usleep(10000);
+			}
+		}
+	}
+#endif
+
 	/**
 	 * whether logging is currently active or not (any of the selected backends).
 	 */
@@ -90,15 +103,16 @@ class LogWriter
 	 *         -1 if not enough space in the buffer left (file backend), -2 mavlink backend failed
 	 *  add type -> pass through, but not to mavlink if mission log
 	 */
-	int write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start = 0);
+	int write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start = 0, bool reliable = false,
+			  bool wait = false);
 
 	/**
 	 * Select a backend, so that future calls to write_message() only write to the selected
 	 * sel_backend, until unselect_write_backend() is called.
 	 * @param backend
 	 */
 	void select_write_backend(Backend sel_backend);
-	void unselect_write_backend() { select_write_backend(BackendAll); }
+	void unselect_write_backend() { select_write_backend(_backend); }
 
 	/* file logging methods */
 
@@ -154,7 +168,11 @@ class LogWriter
 	{
 		if (_log_writer_file) { _log_writer_file->set_need_reliable_transfer(need_reliable); }
 
+#ifndef LOGGER_PARALLEL_LOGGING
+
 		if (_log_writer_mavlink) { _log_writer_mavlink->set_need_reliable_transfer(need_reliable && mavlink_backed_too); }
+
+#endif
 	}
 
 	bool need_reliable_transfer() const
@@ -165,7 +183,6 @@ class LogWriter
 
 		return false;
 	}
-
 #if defined(PX4_CRYPTO)
 	void set_encryption_parameters(px4_crypto_algorithm_t algorithm, uint8_t key_idx,  uint8_t exchange_key_idx)
 	{