diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index ca3a6b5301..6cee634102 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -38,4 +38,5 @@ QURT_LIB(LIB_NAME px4 ${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c LINK_LIBS ${module_libraries} + px4_layer ) diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index ae7babf0b7..9bd2ebb7eb 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -32,3 +32,10 @@ ############################################################################ # Placeholder +set(QURT_LAYER_SRCS + tasks.cpp + ) + +add_library(px4_layer + ${QURT_LAYER_SRCS} +) diff --git a/platforms/qurt/src/px4/tasks.cpp b/platforms/qurt/src/px4/tasks.cpp new file mode 100644 index 0000000000..2dd044da44 --- /dev/null +++ b/platforms/qurt/src/px4/tasks.cpp @@ -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 +#include +#include +#include + +#include + +#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(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; +} diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index 03ff927a93..3916683d58 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -30,14 +30,13 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + #include "uORBProtobufChannel.hpp" #include "MUORBTest.hpp" #include -#include -#include #include - +#include #include // 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)