fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4_work_queue: Use px4_task_spawn_cmd for WorkQueueRunner in NuttX protected

Browse Source 
In NuttX protected build there are separate work queues in kernel and user sides.

pthreads are only available in user side, so use tasks and kthreads for
memory protected builds.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen 2021-01-25 09:16:23 +02:00 committed by Daniel Agar
parent 3aab4d2daf
commit 77af102cab
1 changed files with 56 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
platforms/common/px4_work_queue/WorkQueueManager.cpp
View File

@ -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, &param);
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, &param);
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, &param);
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
}
}