diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index b4efae40af..9263cbd926 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -235,6 +235,17 @@ WorkQueueRunner(void *context) return nullptr; } +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +// Wrapper for px4_task_spawn_cmd interface +inline static int +WorkQueueRunner(int argc, char *argv[]) +{ + wq_config_t *context = (wq_config_t *)strtoul(argv[argc - 1], nullptr, 16); + WorkQueueRunner(context); + return 0; +} +#endif + static int WorkQueueManagerRun(int, char **) { @@ -248,20 +259,6 @@ WorkQueueManagerRun(int, char **) if (wq != nullptr) { // create new work queue - pthread_attr_t attr; - int ret_attr_init = pthread_attr_init(&attr); - - if (ret_attr_init != 0) { - PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init); - } - - sched_param param; - int ret_getschedparam = pthread_attr_getschedparam(&attr, ¶m); - - if (ret_getschedparam != 0) { - PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam); - } - // stack size #if defined(__PX4_QURT) const size_t stacksize = math::max(8 * 1024, PX4_STACK_ADJUSTED(wq->stacksize)); @@ -274,12 +271,32 @@ WorkQueueManagerRun(int, char **) const size_t stacksize_adj = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize)); const size_t stacksize = (stacksize_adj + page_size - (stacksize_adj % page_size)); #endif + + // priority + int sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority; + + // use pthreads for NuttX flat and posix builds. For NuttX protected build, use tasks or kernel threads +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) + pthread_attr_t attr; + int ret_attr_init = pthread_attr_init(&attr); + int ret_setstacksize = pthread_attr_setstacksize(&attr, stacksize); if (ret_setstacksize != 0) { PX4_ERR("setting stack size for %s failed (%i)", wq->name, ret_setstacksize); } + if (ret_attr_init != 0) { + PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init); + } + + sched_param param; + int ret_getschedparam = pthread_attr_getschedparam(&attr, ¶m); + + if (ret_getschedparam != 0) { + PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam); + } + #ifndef __PX4_QURT // schedule policy FIFO @@ -292,7 +309,7 @@ WorkQueueManagerRun(int, char **) #endif // ! QuRT // priority - param.sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority; + param.sched_priority = sched_priority; int ret_setschedparam = pthread_attr_setschedparam(&attr, ¶m); if (ret_setschedparam != 0) { @@ -316,6 +333,30 @@ WorkQueueManagerRun(int, char **) if (ret_destroy != 0) { PX4_ERR("failed to destroy thread attributes for %s (%i)", wq->name, ret_create); } + +#else + // create thread + + // pack wq struct pointer into string, this is compatible with px4_task_spawn_cmd + char arg1[sizeof(void *) * 3]; + sprintf(arg1, "%lx", (long unsigned)wq); + const char *arg[2] = {arg1, nullptr}; + + int pid = px4_task_spawn_cmd(wq->name, + SCHED_FIFO, + sched_priority, + stacksize, + WorkQueueRunner, + (char *const *)arg); + + if (pid > 0) { + PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, sched_priority, stacksize); + + } else { + PX4_ERR("failed to create thread for %s (%i): %s", wq->name, pid, strerror(pid)); + } + +#endif } }