From 2f7ca3b815151688fdfe3a2112e1804e86d9c868 Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Mon, 11 Dec 2023 13:08:50 +0200
Subject: [PATCH] uORBManager: Automatically set callback thread priority in
 kernel / posix builds

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 platforms/common/uORB/uORBManager.cpp | 42 ++++++++++++++++++++++-----
 platforms/common/uORB/uORBManager.hpp |  7 ++---
 2 files changed, 37 insertions(+), 12 deletions(-)

diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp
index 46d43df3beeb..21dee925c54a 100644
--- a/platforms/common/uORB/uORBManager.cpp
+++ b/platforms/common/uORB/uORBManager.cpp
@@ -65,6 +65,7 @@ uORB::Manager *uORB::Manager::_Instance = nullptr;
 #ifndef CONFIG_BUILD_FLAT
 int8_t uORB::Manager::per_process_lock = -1;
 pid_t uORB::Manager::per_process_cb_thread = -1;
+int uORB::Manager::per_process_cb_priority;
 List<class uORB::SubscriptionCallback *> uORB::Manager::per_process_cb_list;
 pthread_mutex_t uORB::Manager::per_process_cb_list_mutex = PTHREAD_MUTEX_INITIALIZER;
 #endif
@@ -460,19 +461,23 @@ int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeo
 #ifndef CONFIG_BUILD_FLAT
 
 int8_t
-uORB::Manager::launchCallbackThread()
+uORB::Manager::getCallbackLock()
 {
-	per_process_lock = Manager::getThreadLock();
+	pthread_mutex_lock(&per_process_cb_list_mutex);
 
-	if (per_process_lock < 0) {
-		PX4_ERR("Out of thread locks\n");
-		return -1;
-	}
+	int priority = px4_get_process_max_priority();
 
 	if (per_process_cb_thread == -1) {
+		per_process_lock = Manager::getThreadLock();
+
+		if (per_process_lock < 0) {
+			PX4_ERR("Out of thread locks\n");
+			return -1;
+		}
+
 		per_process_cb_thread = px4_task_spawn_cmd("orb_callback",
 					SCHED_DEFAULT,
-					SCHED_PRIORITY_MAX - 1,
+					priority,
 					PX4_STACK_ADJUSTED(1024),
 					callback_thread,
 					nullptr);
@@ -482,8 +487,31 @@ uORB::Manager::launchCallbackThread()
 			Manager::freeThreadLock(per_process_lock);
 			return -1;
 		}
+
+		per_process_cb_priority = priority;
+
+	} else if (priority > per_process_cb_priority) {
+		sched_param param;
+		int policy;
+		int schedparam_ret = pthread_getschedparam(per_process_cb_thread, &policy, &param);
+
+		if (schedparam_ret) {
+			PX4_ERR("getschedparam fail, can't set priority\n");
+
+		} else {
+			param.sched_priority = priority;
+
+			if (pthread_setschedparam(per_process_cb_thread, policy, &param)) {
+				PX4_ERR("setschedparam fail\n");
+
+			} else {
+				per_process_cb_priority = priority;
+			}
+		}
 	}
 
+	pthread_mutex_unlock(&per_process_cb_list_mutex);
+
 	return per_process_lock;
 }
 
diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp
index 51542bbdf94a..7675e0d679fa 100644
--- a/platforms/common/uORB/uORBManager.hpp
+++ b/platforms/common/uORB/uORBManager.hpp
@@ -369,10 +369,7 @@ class Manager
 	}
 
 #ifndef CONFIG_BUILD_FLAT
-	static uint8_t getCallbackLock()
-	{
-		return per_process_lock >= 0 ? per_process_lock : launchCallbackThread();
-	}
+	static int8_t getCallbackLock();
 #endif
 
 	static uint8_t orb_get_instance(orb_advert_t &node_handle)
@@ -466,7 +463,6 @@ class Manager
 
 	static void cleanup();
 	static int callback_thread(int argc, char *argv[]);
-	static int8_t launchCallbackThread();
 
 private: // data members
 	static Manager *_Instance;
@@ -670,6 +666,7 @@ class Manager
 #ifndef CONFIG_BUILD_FLAT
 	static int8_t per_process_lock;
 	static pid_t per_process_cb_thread;
+	static int per_process_cb_priority;
 	static List<class SubscriptionCallback *> per_process_cb_list;
 	static pthread_mutex_t per_process_cb_list_mutex;
 #endif