int UavcanServers::start(uavcan::INode &main_node)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* Node init
*/
_instance = new UavcanServers(main_node);
if (_instance == nullptr) {
warnx("Out of memory");
return -2;
}
int rv = _instance->init();
if (rv < 0) {
warnx("Node init failed: %d", rv);
delete _instance;
_instance = nullptr;
return rv;
}
/*
* Start the thread. Normally it should never exit.
*/
pthread_attr_t tattr;
struct sched_param param;
pthread_attr_init(&tattr);
(void)pthread_attr_getschedparam(&tattr, ¶m);
tattr.stacksize = PX4_STACK_ADJUSTED(StackSize);
param.sched_priority = Priority;
if (pthread_attr_setschedparam(&tattr, ¶m)) {
warnx("setting sched params failed");
}
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
if (rv != 0) {
rv = -rv;
warnx("pthread_create() failed: %d", rv);
delete _instance;
_instance = nullptr;
}
return rv;
}
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;
int i;
unsigned int len = 0;
unsigned long offset;
unsigned long structsize;
char *p = (char *)argv;
pthread_attr_t attr;
struct sched_param param = {};
// Calculate argc
while (p != (char *)nullptr) {
p = argv[argc];
if (p == (char *)nullptr) {
break;
}
++argc;
len += strlen(p) + 1;
}
structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *);
// not safe to pass stack data to the thread creation
pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len);
memset(taskdata, 0, structsize + len);
offset = ((unsigned long)taskdata) + structsize;
strncpy(taskdata->name, name, 16);
taskdata->name[15] = 0;
taskdata->entry = entry;
taskdata->argc = argc;
for (i = 0; i < argc; i++) {
PX4_DEBUG("arg %d %s\n", i, argv[i]);
taskdata->argv[i] = (char *)offset;
strcpy((char *)offset, argv[i]);
offset += strlen(argv[i]) + 1;
}
// Must add NULL at end of argv
taskdata->argv[argc] = (char *)nullptr;
PX4_DEBUG("starting task %s", name);
rv = pthread_attr_init(&attr);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to init thread attrs");
free(taskdata);
return (rv < 0) ? rv : -rv;
}
#ifndef __PX4_DARWIN
if (stack_size < PTHREAD_STACK_MIN) {
stack_size = PTHREAD_STACK_MIN;
}
rv = pthread_attr_setstacksize(&attr, PX4_STACK_ADJUSTED(stack_size));
if (rv != 0) {
PX4_ERR("pthread_attr_setstacksize to %d returned error (%d)", stack_size, rv);
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
#endif
rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to set inherit sched");
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
rv = pthread_attr_setschedpolicy(&attr, scheduler);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to set sched policy");
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
param.sched_priority = priority;
rv = pthread_attr_setschedparam(&attr, ¶m);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to set sched param");
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
pthread_mutex_lock(&task_mutex);
int taskid = 0;
for (i = 0; i < PX4_MAX_TASKS; ++i) {
if (taskmap[i].isused == false) {
taskmap[i].name = name;
taskmap[i].isused = true;
taskid = i;
break;
}
}
if (i >= PX4_MAX_TASKS) {
pthread_attr_destroy(&attr);
pthread_mutex_unlock(&task_mutex);
free(taskdata);
return -ENOSPC;
}
rv = pthread_create(&taskmap[taskid].pid, &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(&taskmap[taskid].pid, nullptr, &entry_adapter, (void *) taskdata);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
taskmap[taskid].isused = false;
pthread_attr_destroy(&attr);
pthread_mutex_unlock(&task_mutex);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
} else {
pthread_attr_destroy(&attr);
pthread_mutex_unlock(&task_mutex);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
}
pthread_attr_destroy(&attr);
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[])
{
struct sched_param param;
pthread_attr_t attr;
pthread_t task;
int rv;
int argc = 0;
int i;
unsigned int len = 0;
unsigned long offset;
unsigned long structsize;
char *p = (char *)argv;
PX4_DEBUG("Creating %s\n", name);
PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m);
// Calculate argc
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 = nullptr;
// not safe to pass stack data to the thread creation
taskdata = (pthdata_t *)malloc(structsize + len);
offset = ((unsigned long)taskdata) + structsize;
taskdata->entry = entry;
taskdata->argc = argc;
for (i = 0; i < argc; i++) {
PX4_DEBUG("arg %d %s\n", i, argv[i]);
taskdata->argv[i] = (char *)offset;
strcpy((char *)offset, argv[i]);
offset += strlen(argv[i]) + 1;
}
// 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;
}
PX4_DEBUG("stack address after pthread_attr_init: 0x%X", attr.stackaddr);
PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m);
rv = pthread_attr_getschedparam(&attr, ¶m);
PX4_DEBUG("stack address after pthread_attr_getschedparam: 0x%X", attr.stackaddr);
if (rv != 0) {
PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param");
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;
}
#endif
size_t fixed_stacksize = -1;
pthread_attr_getstacksize(&attr, &fixed_stacksize);
PX4_DEBUG("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size);
fixed_stacksize = 8 * 1024;
fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize;
PX4_DEBUG("setting the thread[%s] stack size to[%d]", name, fixed_stacksize);
pthread_attr_setstacksize(&attr, PX4_STACK_ADJUSTED(fixed_stacksize));
PX4_DEBUG("stack address after pthread_attr_setstacksize: 0x%X", attr.stackaddr);
param.sched_priority = priority;
rv = pthread_attr_setschedparam(&attr, ¶m);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to set sched param");
return (rv < 0) ? rv : -rv;
}
rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: pthread_create failed, error: %d", rv);
return (rv < 0) ? rv : -rv;
}
for (i = 0; i < PX4_MAX_TASKS; ++i) {
if (taskmap[i].isused == false) {
taskmap[i].pid = task;
taskmap[i].name = name;
taskmap[i].isused = true;
break;
}
}
if (i >= PX4_MAX_TASKS) {
return -ENOSPC;
}
return i;
}
