mirror of https://github.com/ArduPilot/ardupilot
HAL_PX4: implement thread_create() API
This commit is contained in:
parent
df3ce87e02
commit
3dfa1f2879
|
@ -446,4 +446,68 @@ void PX4Scheduler::restore_interrupts(void *state)
|
|||
irqrestore((irqstate_t)(uintptr_t)state);
|
||||
}
|
||||
|
||||
/*
|
||||
trampoline for thread create
|
||||
*/
|
||||
void *PX4Scheduler::thread_create_trampoline(void *ctx)
|
||||
{
|
||||
AP_HAL::MemberProc *t = (AP_HAL::MemberProc *)ctx;
|
||||
(*t)();
|
||||
free(t);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/*
|
||||
create a new thread
|
||||
*/
|
||||
bool PX4Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
|
||||
{
|
||||
// take a copy of the MemberProc, it is freed after thread exits
|
||||
AP_HAL::MemberProc *tproc = (AP_HAL::MemberProc *)malloc(sizeof(proc));
|
||||
if (!tproc) {
|
||||
return false;
|
||||
}
|
||||
*tproc = proc;
|
||||
|
||||
uint8_t thread_priority = APM_IO_PRIORITY;
|
||||
static const struct {
|
||||
priority_base base;
|
||||
uint8_t p;
|
||||
} priority_map[] = {
|
||||
{ PRIORITY_BOOST, APM_MAIN_PRIORITY_BOOST},
|
||||
{ PRIORITY_MAIN, APM_MAIN_PRIORITY},
|
||||
{ PRIORITY_SPI, APM_SPI_PRIORITY},
|
||||
{ PRIORITY_I2C, APM_I2C_PRIORITY},
|
||||
{ PRIORITY_CAN, APM_CAN_PRIORITY},
|
||||
{ PRIORITY_TIMER, APM_TIMER_PRIORITY},
|
||||
{ PRIORITY_RCIN, APM_TIMER_PRIORITY},
|
||||
{ PRIORITY_IO, APM_IO_PRIORITY},
|
||||
{ PRIORITY_UART, APM_UART_PRIORITY},
|
||||
{ PRIORITY_STORAGE, APM_STORAGE_PRIORITY},
|
||||
};
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(priority_map); i++) {
|
||||
if (priority_map[i].base == base) {
|
||||
thread_priority = constrain_int16(priority_map[i].p + priority, 1, APM_MAX_PRIORITY);
|
||||
break;
|
||||
}
|
||||
}
|
||||
pthread_t thread;
|
||||
pthread_attr_t thread_attr;
|
||||
struct sched_param param;
|
||||
|
||||
pthread_attr_init(&thread_attr);
|
||||
pthread_attr_setstacksize(&thread_attr, stack_size);
|
||||
|
||||
param.sched_priority = thread_priority;
|
||||
(void)pthread_attr_setschedparam(&thread_attr, ¶m);
|
||||
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
|
||||
|
||||
if (pthread_create(&thread, &thread_attr, thread_create_trampoline, tproc) != 0) {
|
||||
free(tproc);
|
||||
return false;
|
||||
}
|
||||
pthread_setname_np(thread, name);
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -10,6 +10,7 @@
|
|||
|
||||
#define PX4_SCHEDULER_MAX_TIMER_PROCS 8
|
||||
|
||||
#define APM_MAX_PRIORITY 243
|
||||
#define APM_MAIN_PRIORITY_BOOST 241
|
||||
#define APM_MAIN_PRIORITY 180
|
||||
#define APM_TIMER_PRIORITY 181
|
||||
|
@ -72,6 +73,11 @@ public:
|
|||
restore interrupt state from disable_interrupts_save()
|
||||
*/
|
||||
void restore_interrupts(void *) override;
|
||||
|
||||
/*
|
||||
create a new thread
|
||||
*/
|
||||
bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override;
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
|
@ -115,5 +121,6 @@ private:
|
|||
perf_counter_t _perf_io_timers;
|
||||
perf_counter_t _perf_storage_timer;
|
||||
perf_counter_t _perf_delay;
|
||||
static void *thread_create_trampoline(void *ctx);
|
||||
};
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue