|
|
|
@ -10,6 +10,7 @@
@@ -10,6 +10,7 @@
|
|
|
|
|
#include <unistd.h> |
|
|
|
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h> |
|
|
|
|
|
|
|
|
|
#include "RCInput.h" |
|
|
|
|
#include "RPIOUARTDriver.h" |
|
|
|
@ -82,18 +83,16 @@ void Scheduler::init()
@@ -82,18 +83,16 @@ void Scheduler::init()
|
|
|
|
|
SCHED_THREAD(io, IO), |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
#if !APM_BUILD_TYPE(APM_BUILD_Replay) |
|
|
|
|
// we don't run Replay in real-time...
|
|
|
|
|
mlockall(MCL_CURRENT|MCL_FUTURE); |
|
|
|
|
|
|
|
|
|
if (geteuid() != 0) { |
|
|
|
|
printf("WARNING: running as non-root. Will not use realtime scheduling\n"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
struct sched_param param = { .sched_priority = APM_LINUX_MAIN_PRIORITY }; |
|
|
|
|
ret = sched_setscheduler(0, SCHED_FIFO, ¶m); |
|
|
|
|
if (ret == -1) { |
|
|
|
|
if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { |
|
|
|
|
AP_HAL::panic("Scheduler: failed to set scheduling parameters: %s", |
|
|
|
|
strerror(errno)); |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
/* set barrier to N + 1 threads: worker threads + main */ |
|
|
|
|
unsigned n_threads = ARRAY_SIZE(sched_table) + 1; |
|
|
|
|