Logger: add watchdog to boost its priority in case of a busy-looping task

src/modules/logger/CMakeLists.txt

@@ -43,6 +43,7 @@ px4_add_module(
 		log_writer.cpp
 		log_writer_file.cpp
 		log_writer_mavlink.cpp
+		watchdog.cpp
 	DEPENDS
 		platforms__common
 		modules__uORB

src/modules/logger/log_writer.h

@@ -137,6 +137,13 @@ class LogWriter
 		return 0;
 	}
 
+	pthread_t thread_id_file() const
+	{
+		if (_log_writer_file) { return _log_writer_file->thread_id(); }
+
+		return (pthread_t)0;
+	}
+
 
 	/**
 	 * Indicate to the underlying backend whether future write_message() calls need a reliable

src/modules/logger/log_writer_file.cpp

@@ -42,6 +42,9 @@
 #include <systemlib/hardfault_log.h>
 #endif /* __PX4_NUTTX */
 
+using namespace time_literals;
+
+
 namespace px4
 {
 namespace logger
@@ -229,6 +232,7 @@ void LogWriterFile::run()
 
 		int poll_count = 0;
 		int written = 0;
+		hrt_abstime last_fsync = hrt_absolute_time();
 
 		while (true) {
 			size_t available = 0;
@@ -263,12 +267,15 @@ void LogWriterFile::run()
 				written = ::write(_fd, read_ptr, available);
 				perf_end(_perf_write);
 
+				const hrt_abstime now = hrt_absolute_time();
+
 				/* call fsync periodically to minimize potential loss of data */
-				if (++poll_count >= 100) {
+				if (++poll_count >= 100 || now - last_fsync > 1_s) {
 					perf_begin(_perf_fsync);
 					::fsync(_fd);
 					perf_end(_perf_fsync);
 					poll_count = 0;
+					last_fsync = now;
 				}
 
 				if (written < 0) {

src/modules/logger/log_writer_file.h

@@ -113,6 +113,8 @@ class LogWriterFile
 		return _need_reliable_transfer;
 	}
 
+	pthread_t thread_id() const { return _thread; }
+
 private:
 	static void *run_helper(void *);
 

src/modules/logger/logger.cpp

@@ -34,6 +34,7 @@
 #include <px4_config.h>
 #include "logger.h"
 #include "messages.h"
+#include "watchdog.h"
 
 #include <dirent.h>
 #include <sys/stat.h>
@@ -88,10 +89,24 @@
 
 using namespace px4::logger;
 
+
+struct timer_callback_data_s {
+	px4_sem_t semaphore;
+
+	watchdog_data_t watchdog_data;
+	volatile bool watchdog_triggered = false;
+};
+
 /* This is used to schedule work for the logger (periodic scan for updated topics) */
 static void timer_callback(void *arg)
 {
-	px4_sem_t *semaphore = (px4_sem_t *)arg;
+	/* Note: we are in IRQ context here (on NuttX) */
+
+	timer_callback_data_s *data = (timer_callback_data_s *)arg;
+
+	if (watchdog_update(data->watchdog_data)) {
+		data->watchdog_triggered = true;
+	}
 
 	/* check the value of the semaphore: if the logger cannot keep up with running it's main loop as fast
 	 * as the timer_callback here increases the semaphore count, the counter would increase unbounded,
@@ -101,11 +116,12 @@ static void timer_callback(void *arg)
 	 * multiple iterations at once, the next time it's scheduled). */
 	int semaphore_value;
 
-	if (px4_sem_getvalue(semaphore, &semaphore_value) == 0 && semaphore_value > 1) {
+	if (px4_sem_getvalue(&data->semaphore, &semaphore_value) == 0 && semaphore_value > 1) {
 		return;
 	}
 
-	px4_sem_post(semaphore);
+	px4_sem_post(&data->semaphore);
+
 }
 
 
@@ -922,12 +938,11 @@ void Logger::run()
 	}
 
 	/* init the update timer */
-	struct hrt_call timer_call;
-	memset(&timer_call, 0, sizeof(hrt_call));
-	px4_sem_t timer_semaphore;
-	px4_sem_init(&timer_semaphore, 0, 0);
+	struct hrt_call timer_call{};
+	timer_callback_data_s timer_callback_data;
+	px4_sem_init(&timer_callback_data.semaphore, 0, 0);
 	/* timer_semaphore use case is a signal */
-	px4_sem_setprotocol(&timer_semaphore, SEM_PRIO_NONE);
+	px4_sem_setprotocol(&timer_callback_data.semaphore, SEM_PRIO_NONE);
 
 	int polling_topic_sub = -1;
 
@@ -939,7 +954,18 @@ void Logger::run()
 		}
 
 	} else {
-		hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_semaphore);
+
+		if (_writer.backend() & LogWriter::BackendFile) {
+
+			const pid_t pid_self = getpid();
+			const pthread_t writer_thread = _writer.thread_id_file();
+
+			// sched_note_start is already called from pthread_create and task_create,
+			// which means we can expect to find the tasks in system_load.tasks, as required in watchdog_initialize
+			watchdog_initialize(pid_self, writer_thread, timer_callback_data.watchdog_data);
+		}
+
+		hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_callback_data);
 	}
 
 	// check for new subscription data
@@ -976,7 +1002,7 @@ void Logger::run()
 
 				} else {
 					// delayed stop: we measure the process loads and then stop
-					initialize_load_output();
+					initialize_load_output(PrintLoadReason::Postflight);
 					_should_stop_file_log = true;
 				}
 			}
@@ -1015,6 +1041,11 @@ void Logger::run()
 		}
 
 
+		if (timer_callback_data.watchdog_triggered) {
+			timer_callback_data.watchdog_triggered = false;
+			initialize_load_output(PrintLoadReason::Watchdog);
+		}
+
 
 		const hrt_abstime loop_time = hrt_absolute_time();
 
@@ -1026,12 +1057,12 @@ void Logger::run()
 
 				if (_should_stop_file_log) {
 					_should_stop_file_log = false;
-					write_load_output(false);
+					write_load_output();
 					stop_log_file();
 					continue; // skip to next loop iteration
 
 				} else {
-					write_load_output(true);
+					write_load_output();
 				}
 			}
 
@@ -1200,14 +1231,14 @@ void Logger::run()
 			 * And on linux this is quite accurate as well, but under NuttX it is not accurate,
 			 * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms).
 			 */
-			while (px4_sem_wait(&timer_semaphore) != 0);
+			while (px4_sem_wait(&timer_callback_data.semaphore) != 0);
 		}
 	}
 
 	stop_log_file();
 
 	hrt_cancel(&timer_call);
-	px4_sem_destroy(&timer_semaphore);
+	px4_sem_destroy(&timer_callback_data.semaphore);
 
 	// stop the writer thread
 	_writer.thread_stop();
@@ -1482,7 +1513,7 @@ void Logger::start_log_file()
 
 	_start_time_file = hrt_absolute_time();
 
-	initialize_load_output();
+	initialize_load_output(PrintLoadReason::Preflight);
 }
 
 void Logger::stop_log_file()
@@ -1518,7 +1549,7 @@ void Logger::start_log_mavlink()
 	_writer.unselect_write_backend();
 	_writer.notify();
 
-	initialize_load_output();
+	initialize_load_output(PrintLoadReason::Preflight);
 }
 
 void Logger::stop_log_mavlink()
@@ -1576,18 +1607,26 @@ void Logger::print_load_callback(void *user)
 		return;
 	}
 
-	if (callback_data->preflight) {
-		perf_name = "perf_top_preflight";
-
-	} else {
-		perf_name = "perf_top_postflight";
+	switch (callback_data->logger->_print_load_reason) {
+		case PrintLoadReason::Preflight:
+			perf_name = "perf_top_preflight";
+			break;
+		case PrintLoadReason::Postflight:
+			perf_name = "perf_top_postflight";
+			break;
+		case PrintLoadReason::Watchdog:
+			perf_name = "perf_top_watchdog";
+			break;
+		default:
+			perf_name = "perf_top";
+			break;
 	}
 
 	callback_data->logger->write_info_multiple(perf_name, callback_data->buffer, callback_data->counter != 0);
 	++callback_data->counter;
 }
 
-void Logger::initialize_load_output()
+void Logger::initialize_load_output(PrintLoadReason reason)
 {
 	perf_callback_data_t callback_data;
 	callback_data.logger = this;
@@ -1599,16 +1638,19 @@ void Logger::initialize_load_output()
 	// this will not yet print anything
 	print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load);
 	_next_load_print = curr_time + 1000000;
+	_print_load_reason = reason;
 }
 
-void Logger::write_load_output(bool preflight)
+void Logger::write_load_output()
 {
+	if (_print_load_reason == PrintLoadReason::Watchdog) {
+		PX4_ERR("Writing watchdog data"); // this is just that we see it easily in the log
+	}
 	perf_callback_data_t callback_data = {};
 	char buffer[140];
 	callback_data.logger = this;
 	callback_data.counter = 0;
 	callback_data.buffer = buffer;
-	callback_data.preflight = preflight;
 	hrt_abstime curr_time = hrt_absolute_time();
 	_writer.set_need_reliable_transfer(true);
 	// TODO: maybe we should restrict the output to a selected backend (eg. when file logging is running

src/modules/logger/logger.h

@@ -161,6 +161,12 @@ class Logger : public ModuleBase<Logger>
 
 private:
 
+	enum class PrintLoadReason {
+		Preflight,
+		Postflight,
+		Watchdog
+	};
+
 	/**
 	 * Write an ADD_LOGGED_MSG to the log for a all current subscriptions and instances
 	 */
@@ -297,12 +303,12 @@ class Logger : public ModuleBase<Logger>
 	/**
 	 * initialize the output for the process load, so that ~1 second later it will be written to the log
 	 */
-	void initialize_load_output();
+	void initialize_load_output(PrintLoadReason reason);
 
 	/**
 	 * write the process load, which was previously initialized with initialize_load_output()
 	 */
-	void write_load_output(bool preflight);
+	void write_load_output();
 
 
 	static constexpr size_t 	MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */
@@ -344,6 +350,7 @@ class Logger : public ModuleBase<Logger>
 											will be stopped after load printing */
 	print_load_s					_load{}; ///< process load data
 	hrt_abstime					_next_load_print{0}; ///< timestamp when to print the process load
+	PrintLoadReason					_print_load_reason;
 
 	// control
 	param_t						_sdlog_profile_handle{PARAM_INVALID};