uORBManager: Automatically set callback thread priority in kernel / p…

…osix builds Signed-off-by: Jukka Laitinen <[email protected]>
tiiuae · Dec 12, 2023 · 2f7ca3b · 2f7ca3b
1 parent 88ab467
commit 2f7ca3b
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 12 deletions.
diff --git 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
@@ -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