void *VRBRAINScheduler::_timer_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
uint32_t last_ran_overtime = 0;
pthread_setname_np(pthread_self(), "apm_timer");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(1000);
// run registered timers
perf_begin(sched->_perf_timers);
sched->_run_timers();
perf_end(sched->_perf_timers);
// process any pending RC output requests
hal.rcout->timer_tick();
// process any pending RC input requests
((VRBRAINRCInput *)hal.rcin)->_timer_tick();
if (vrbrain_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) {
last_ran_overtime = AP_HAL::millis();
#if 0
printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
#endif
}
}
return nullptr;
}
void *VRBRAINScheduler::_io_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
while (!sched->_hal_initialized) {
poll(NULL, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
poll(NULL, 0, 1);
// run registered IO processes
perf_begin(sched->_perf_io_timers);
sched->_run_io();
perf_end(sched->_perf_io_timers);
}
return NULL;
}
void *VRBRAINScheduler::_storage_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
pthread_setname_np(pthread_self(), "apm_storage");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(10000);
// process any pending storage writes
perf_begin(sched->_perf_storage_timer);
hal.storage->_timer_tick();
perf_end(sched->_perf_storage_timer);
}
return nullptr;
}
void *VRBRAINScheduler::_io_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
pthread_setname_np(pthread_self(), "apm_io");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(1000);
// run registered IO processes
perf_begin(sched->_perf_io_timers);
sched->_run_io();
perf_end(sched->_perf_io_timers);
}
return nullptr;
}
void *VRBRAINScheduler::_uart_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
while (!sched->_hal_initialized) {
poll(NULL, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(1000);
// process any pending serial bytes
((VRBRAINUARTDriver *)hal.uartA)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartB)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartC)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartD)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartE)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartF)->_timer_tick();
}
return NULL;
}
void *VRBRAINScheduler::_uart_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
pthread_setname_np(pthread_self(), "apm_uart");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(1000);
// process any pending serial bytes
hal.uartA->_timer_tick();
hal.uartB->_timer_tick();
hal.uartC->_timer_tick();
hal.uartD->_timer_tick();
hal.uartE->_timer_tick();
hal.uartF->_timer_tick();
}
return nullptr;
}
static int main_loop(int argc, char **argv)
{
extern void setup(void);
extern void loop(void);
hal.uartA->begin(115200);
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.uartD->begin(57600);
hal.uartE->begin(57600);
hal.scheduler->init(NULL);
hal.rcin->init(NULL);
hal.rcout->init(NULL);
hal.analogin->init(NULL);
hal.gpio->init();
/*
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
*/
set_priority(APM_STARTUP_PRIORITY);
schedulerInstance.hal_initialized();
setup();
hal.scheduler->system_initialized();
perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");
struct hrt_call loop_overtime_call;
thread_running = true;
/*
switch to high priority for main loop
*/
set_priority(APM_MAIN_PRIORITY);
while (!_vrbrain_thread_should_exit) {
perf_begin(perf_loop);
/*
this ensures a tight loop waiting on a lower priority driver
will eventually give up some time for the driver to run. It
will only ever be called if a loop() call runs for more than
0.1 second
*/
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
loop();
if (vrbrain_ran_overtime) {
/*
we ran over 1s in loop(), and our priority was lowered
to let a driver run. Set it back to high priority now.
*/
set_priority(APM_MAIN_PRIORITY);
perf_count(perf_overrun);
vrbrain_ran_overtime = false;
}
perf_end(perf_loop);
/*
give up 500 microseconds of time, to ensure drivers get a
chance to run. This relies on the accurate semaphore wait
using hrt in semaphore.cpp
*/
hal.scheduler->delay_microseconds(500);
}
thread_running = false;
return 0;
}
