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; }