mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
QuRT: Move to POSIX API for threads
Use the pthread APIs to implement task support Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
parent
8dfb09418b
commit
ad1865ef9f
@ -33,10 +33,11 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4_linux_tasks.c
|
||||
* @file px4_qurt_tasks.c
|
||||
* Implementation of existing task API for Linux
|
||||
*/
|
||||
|
||||
#include <px4_debug.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
@ -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; i<argc; i++) {
|
||||
printf("TEST\n");
|
||||
printf("arg %d %s\n", i, argv[i]);
|
||||
taskdata->argv[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; ++i) {
|
||||
if (taskmap[i].isused == false) {
|
||||
taskmap[i].pid = i+1;
|
||||
taskmap[i].pid = task;
|
||||
taskmap[i].name = name;
|
||||
taskmap[i].isused = true;
|
||||
taskmap[i].sp = malloc(stack_size);
|
||||
//taskmap[i].sp = malloc(stack_size);
|
||||
break;
|
||||
}
|
||||
}
|
||||
printf("TEST2\n");
|
||||
thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata);
|
||||
|
||||
return i+1;
|
||||
if (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; ++i) {
|
||||
if (taskmap[i].pid == pid) {
|
||||
taskmap[i].isused = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user