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 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, ¶m); + + 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, ¶m)) { + 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 per_process_cb_list; static pthread_mutex_t per_process_cb_list_mutex; #endif