mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 15:40:31 +08:00
Qurt tasks implementation (#20499)
This commit is contained in:
Zachary Lowell
GitHub
@@ -38,4 +38,5 @@ QURT_LIB(LIB_NAME px4
|
||||
${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
|
||||
LINK_LIBS
|
||||
${module_libraries}
|
||||
px4_layer
|
||||
)
|
||||
|
||||
@@ -32,3 +32,10 @@
|
||||
############################################################################
|
||||
|
||||
# Placeholder
|
||||
set(QURT_LAYER_SRCS
|
||||
tasks.cpp
|
||||
)
|
||||
|
||||
add_library(px4_layer
|
||||
${QURT_LAYER_SRCS}
|
||||
)
|
||||
|
||||
@@ -0,0 +1,376 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 ModalAI, Inc. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
|
||||
#include <pthread.h>
|
||||
|
||||
#define PX4_TASK_STACK_SIZE 8192
|
||||
#define PX4_TASK_MAX_NAME_LENGTH 32
|
||||
#define PX4_TASK_MAX_ARGC 32
|
||||
#define PX4_TASK_MAX_ARGV_LENGTH 32
|
||||
#define PX4_MAX_TASKS 24
|
||||
|
||||
typedef struct task_entry {
|
||||
pthread_t tid;
|
||||
char name[PX4_TASK_MAX_NAME_LENGTH + 4];
|
||||
char stack[PX4_TASK_STACK_SIZE];
|
||||
pthread_attr_t attr;
|
||||
px4_main_t main_entry;
|
||||
int argc;
|
||||
char argv_storage[PX4_TASK_MAX_ARGC][PX4_TASK_MAX_ARGV_LENGTH];
|
||||
char *argv[PX4_TASK_MAX_ARGC];
|
||||
bool isused;
|
||||
|
||||
task_entry() : isused(false) {}
|
||||
} task_entry_t;
|
||||
|
||||
static task_entry_t taskmap[PX4_MAX_TASKS];
|
||||
|
||||
static bool task_mutex_initialized = false;
|
||||
static pthread_mutex_t task_mutex;
|
||||
|
||||
static void *entry_adapter(void *ptr)
|
||||
{
|
||||
task_entry_t *data;
|
||||
data = (task_entry_t *) ptr;
|
||||
|
||||
if (data->main_entry) { data->main_entry(data->argc, data->argv); }
|
||||
|
||||
else { PX4_ERR("No valid task entry points"); }
|
||||
|
||||
pthread_exit(nullptr);
|
||||
return nullptr;
|
||||
|
||||
}
|
||||
|
||||
static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_main_t main_entry, char *const argv[])
|
||||
{
|
||||
int retcode = 0;
|
||||
int i = 0;
|
||||
int task_index = 0;
|
||||
char *p = (char *)argv;
|
||||
|
||||
PX4_INFO("Creating pthread %s\n", name);
|
||||
|
||||
if (task_mutex_initialized == false) {
|
||||
task_mutex_initialized = true;
|
||||
pthread_mutex_init(&task_mutex, nullptr);
|
||||
}
|
||||
|
||||
pthread_mutex_lock(&task_mutex);
|
||||
|
||||
for (task_index = 0; task_index < PX4_MAX_TASKS; task_index++) {
|
||||
if (taskmap[task_index].isused == false) { break; }
|
||||
}
|
||||
|
||||
if (task_index == PX4_MAX_TASKS) {
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
PX4_ERR("Hit maximum number of threads");
|
||||
return -1;
|
||||
}
|
||||
|
||||
taskmap[task_index].argc = 0;
|
||||
|
||||
while (p) {
|
||||
taskmap[task_index].argc++;
|
||||
p = argv[taskmap[task_index].argc];
|
||||
}
|
||||
|
||||
if (taskmap[task_index].argc >= PX4_TASK_MAX_ARGC) {
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
PX4_ERR("Too many arguments for thread %d", taskmap[task_index].argc);
|
||||
return -1;
|
||||
}
|
||||
|
||||
for (i = 0; i < PX4_TASK_MAX_ARGC; i++) {
|
||||
if (i < taskmap[task_index].argc) {
|
||||
int argument_length = strlen(argv[i]);
|
||||
|
||||
if (argument_length >= PX4_TASK_MAX_ARGV_LENGTH) {
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
PX4_ERR("Argument %d is too long %d", i, argument_length);
|
||||
return -1;
|
||||
|
||||
} else {
|
||||
//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
|
||||
taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i];
|
||||
}
|
||||
|
||||
} else {
|
||||
taskmap[task_index].argv[i] = nullptr;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
taskmap[task_index].main_entry = main_entry;
|
||||
|
||||
if ((priority > 255) || (priority < 0)) {
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
PX4_ERR("Invalid priority %d", priority);
|
||||
return -1;
|
||||
}
|
||||
|
||||
priority = 255 - priority;
|
||||
|
||||
if (priority < 5) { priority = 5; }
|
||||
|
||||
if (priority > 250) { priority = 250; }
|
||||
|
||||
if (strlen(name) >= PX4_TASK_MAX_NAME_LENGTH) {
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
PX4_ERR("Task name is too long %s", name);
|
||||
return -1;
|
||||
}
|
||||
|
||||
strcpy(taskmap[task_index].name, "PX4_");
|
||||
strcpy(&taskmap[task_index].name[4], name);
|
||||
|
||||
struct sched_param param;
|
||||
param.sched_priority = priority;
|
||||
|
||||
pthread_attr_init(&taskmap[task_index].attr);
|
||||
//pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name);
|
||||
//pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack);
|
||||
pthread_attr_setstacksize(&taskmap[task_index].attr, PX4_TASK_STACK_SIZE);
|
||||
pthread_attr_setschedparam(&taskmap[task_index].attr, ¶m);
|
||||
|
||||
retcode = pthread_create(&taskmap[task_index].tid, &taskmap[task_index].attr, entry_adapter,
|
||||
(void *) &taskmap[task_index]);
|
||||
|
||||
if (retcode != PX4_OK) {
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
PX4_ERR("Couldn't create pthread %s", name);
|
||||
return -1;
|
||||
|
||||
} else {
|
||||
PX4_INFO("Successfully created px4 task %s with tid %u",
|
||||
taskmap[task_index].name,
|
||||
(unsigned int) taskmap[task_index].tid);
|
||||
}
|
||||
|
||||
taskmap[task_index].isused = true;
|
||||
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
|
||||
char *const argv[])
|
||||
{
|
||||
if (entry == nullptr) {
|
||||
PX4_ERR("Entry function pointer is null");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return px4_task_spawn_internal(name, priority, entry, argv);
|
||||
}
|
||||
|
||||
int px4_task_delete(px4_task_t id)
|
||||
{
|
||||
int rv = 0;
|
||||
|
||||
PX4_ERR("Ignoring px4_task_delete for task %d", id);
|
||||
|
||||
pthread_t pid;
|
||||
PX4_WARN("Called px4_task_delete");
|
||||
|
||||
if (id < PX4_MAX_TASKS && taskmap[id].isused) {
|
||||
pid = taskmap[id].tid;
|
||||
|
||||
} else {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
pthread_mutex_lock(&task_mutex);
|
||||
|
||||
if (pthread_self() == pid) {
|
||||
pthread_join(pid, nullptr);
|
||||
taskmap[id].isused = false;
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
pthread_exit(nullptr);
|
||||
|
||||
} else {
|
||||
rv = pthread_cancel(pid);
|
||||
}
|
||||
|
||||
taskmap[id].isused = false;
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
void px4_task_exit(int ret)
|
||||
{
|
||||
PX4_ERR("Ignoring px4_task_exit with return value %d", ret);
|
||||
|
||||
int i;
|
||||
pthread_t pid = pthread_self();
|
||||
|
||||
for (i = 0; i < PX4_MAX_TASKS; ++i) {
|
||||
if (taskmap[i].tid == pid) {
|
||||
pthread_mutex_lock(&task_mutex);
|
||||
taskmap[i].isused = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (i >= PX4_MAX_TASKS) {
|
||||
PX4_ERR("px4_task_exit: self task not found!");
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("px4_task_exit: %s", taskmap[i].name);
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
|
||||
pthread_exit((void *)(unsigned long)ret);
|
||||
}
|
||||
|
||||
int px4_task_kill(px4_task_t id, int sig)
|
||||
{
|
||||
int rv = 0;
|
||||
pthread_t pid;
|
||||
PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name);
|
||||
|
||||
if (id < PX4_MAX_TASKS && taskmap[id].tid != 0) {
|
||||
pthread_mutex_lock(&task_mutex);
|
||||
pid = taskmap[id].tid;
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
|
||||
} else {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
rv = pthread_kill(pid, sig);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
void px4_show_tasks()
|
||||
{
|
||||
int idx = 0;
|
||||
int count = 0;
|
||||
|
||||
PX4_INFO("Active Tasks:");
|
||||
|
||||
for (; idx < PX4_MAX_TASKS; idx++) {
|
||||
if (taskmap[idx].isused) {
|
||||
PX4_INFO(" %-10s %u", taskmap[idx].name,
|
||||
(unsigned int) taskmap[idx].tid);
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
if (count == 0) {
|
||||
PX4_INFO("No running tasks");
|
||||
}
|
||||
}
|
||||
|
||||
px4_task_t px4_getpid()
|
||||
{
|
||||
pthread_t pid = pthread_self();
|
||||
px4_task_t ret = -1;
|
||||
|
||||
pthread_mutex_lock(&task_mutex);
|
||||
|
||||
for (int i = 0; i < PX4_MAX_TASKS; i++) {
|
||||
if (taskmap[i].isused && taskmap[i].tid == pid) {
|
||||
ret = i;
|
||||
}
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
return ret;
|
||||
|
||||
}
|
||||
|
||||
|
||||
const char *px4_get_taskname()
|
||||
{
|
||||
pthread_t pid = pthread_self();
|
||||
const char *prog_name = "UnknownApp";
|
||||
|
||||
pthread_mutex_lock(&task_mutex);
|
||||
|
||||
for (int i = 0; i < PX4_MAX_TASKS; i++) {
|
||||
if (taskmap[i].isused && taskmap[i].tid == pid) {
|
||||
prog_name = taskmap[i].name;
|
||||
}
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&task_mutex);
|
||||
|
||||
return prog_name;
|
||||
|
||||
}
|
||||
|
||||
// static void timer_cb(void *data)
|
||||
// {
|
||||
// px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
|
||||
|
||||
// sem_post(sem);
|
||||
// }
|
||||
|
||||
int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
|
||||
{
|
||||
// work_s _hpwork = {};
|
||||
|
||||
// struct timespec ts_now;
|
||||
// px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
|
||||
|
||||
// hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
|
||||
|
||||
// hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
|
||||
// sem_wait(sem);
|
||||
// hrt_work_cancel(&_hpwork);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int px4_prctl(int option, const char *arg2, pthread_t pid)
|
||||
{
|
||||
int rv = -1;
|
||||
pthread_mutex_lock(&task_mutex);
|
||||
|
||||
for (int i = 0; i < PX4_MAX_TASKS; i++) {
|
||||
if (taskmap[i].isused && taskmap[i].tid == pid) {
|
||||
rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
|
||||
return rv;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
@@ -30,14 +30,13 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "uORBProtobufChannel.hpp"
|
||||
#include "MUORBTest.hpp"
|
||||
#include <string>
|
||||
|
||||
#include <qurt.h>
|
||||
#include <qurt_thread.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
// Definition of test to run when in muorb test mode
|
||||
@@ -45,11 +44,11 @@ static MUORBTestType test_to_run;
|
||||
|
||||
fc_func_ptrs muorb_func_ptrs;
|
||||
|
||||
static void *test_runner(void *test)
|
||||
static void *test_runner(void *)
|
||||
{
|
||||
PX4_INFO("test_runner called");
|
||||
|
||||
switch (*((MUORBTestType *) test)) {
|
||||
switch (test_to_run) {
|
||||
case ADVERTISE_TEST_TYPE:
|
||||
(void) muorb_func_ptrs.advertise_func_ptr(muorb_test_topic_name);
|
||||
break;
|
||||
@@ -93,13 +92,13 @@ char stack[TEST_STACK_SIZE];
|
||||
|
||||
void run_test(MUORBTestType test)
|
||||
{
|
||||
pthread_t tid;
|
||||
pthread_attr_t attr;
|
||||
pthread_attr_init(&attr);
|
||||
pthread_attr_setstacksize(&attr, TEST_STACK_SIZE);
|
||||
test_to_run = test;
|
||||
pthread_create(&tid, &attr, &test_runner, (void *) &test_to_run);
|
||||
pthread_attr_destroy(&attr);
|
||||
(void)px4_task_spawn_cmd("test_MUORB",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 2,
|
||||
2000,
|
||||
(px4_main_t)&test_runner,
|
||||
nullptr);
|
||||
}
|
||||
|
||||
int px4muorb_topic_advertised(const char *topic_name)
|
||||
|
||||
Reference in New Issue
Block a user