diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index b4554bf4fe..456a4a442b 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -33,10 +33,11 @@ ****************************************************************************/ /** - * @file px4_linux_tasks.c + * @file px4_qurt_tasks.c * Implementation of existing task API for Linux */ +#include #include #include #include @@ -60,11 +61,10 @@ #define PX4_MAX_TASKS 100 struct task_entry { - int pid; + pthread_t pid; std::string name; bool isused; task_entry() : isused(false) {} - void *sp; }; static task_entry taskmap[PX4_MAX_TASKS]; @@ -77,28 +77,28 @@ typedef struct // strings are allocated after the } pthdata_t; -static void entry_adapter ( void *ptr ) +static void *entry_adapter ( void *ptr ) { - pthdata_t *data = (pthdata_t *) ptr; + PX4_DBG("entry_adapter\n"); + pthdata_t *data; + data = (pthdata_t *) ptr; - printf("TEST3\n"); -#if 0 - //data->entry(data->argc, data->argv); - printf("TEST4\n"); - printf("Before px4_task_exit\n"); + data->entry(data->argc, data->argv); + free(ptr); + PX4_DBG("Before px4_task_exit"); px4_task_exit(0); - //free(ptr); - printf("After px4_task_exit\n"); -#endif + PX4_DBG("After px4_task_exit"); + + return NULL; } void px4_systemreset(bool to_bootloader) { - printf("Called px4_system_reset\n"); + PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) { int rv; int argc = 0; @@ -106,20 +106,19 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p; + char * p = (char *)argv; + + pthread_t task; + pthread_attr_t attr; + struct sched_param param; - //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); - printf("arg %d %p\n", argc, argv); // Calculate argc - if (argv) { - for(;;) { - p = argv[argc]; - if (p == (char *)0) - break; - printf("arg %d %s\n", argc, argv[argc]); - ++argc; - len += strlen(p)+1; - } + while (p != (char *)0) { + p = argv[argc]; + if (p == (char *)0) + break; + ++argc; + len += strlen(p)+1; } structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; @@ -132,7 +131,6 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskdata->argc = argc; for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); @@ -141,38 +139,124 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; + rv = pthread_attr_init(&attr); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); + return (rv < 0) ? rv : -rv; + } +#if 0 + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); + return (rv < 0) ? rv : -rv; + } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); + return (rv < 0) ? rv : -rv; + } + + param.sched_priority = priority; + + rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); + return (rv < 0) ? rv : -rv; + } +#endif + + rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { + + if (rv == EPERM) { + //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); + rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { + PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + return (rv < 0) ? rv : -rv; + } + } + else { + return (rv < 0) ? rv : -rv; + } + } + for (i=0; i=PX4_MAX_TASKS) { + return -ENOSPC; + } + return i; } int px4_task_delete(px4_task_t id) { - printf("Called px4_task_delete\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_WARN("Called px4_task_delete"); + + if (id < PX4_MAX_TASKS && taskmap[id].isused) + pid = taskmap[id].pid; + else + return -EINVAL; + + // If current thread then exit, otherwise cancel + if (pthread_self() == pid) { + taskmap[id].isused = false; + pthread_exit(0); + } else { + rv = pthread_cancel(pid); + } + + taskmap[id].isused = false; + + return rv; } void px4_task_exit(int ret) { - thread_stop(); + int i; + pthread_t pid = pthread_self(); - // Free stack + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) { + PX4_ERR("px4_task_exit: self task not found!"); + } + else { + PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); + } + + pthread_exit((void *)(unsigned long)ret); } int px4_task_kill(px4_task_t id, int sig) { - printf("Called px4_task_kill\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_DBG("Called px4_task_kill %d", sig); + + if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + pid = taskmap[id].pid; + else + return -EINVAL; + + // If current thread then exit, otherwise cancel + rv = pthread_kill(pid, sig); + + return rv; } void px4_show_tasks() @@ -180,17 +264,16 @@ void px4_show_tasks() int idx; int count = 0; - printf("Active Tasks:\n"); + PX4_INFO("Active Tasks:"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s", taskmap[idx].name.c_str()); count++; } } if (count == 0) - printf(" No running tasks\n"); - + PX4_INFO(" No running tasks"); } // STUBS