void *PX4Scheduler::_timer_thread(void *arg) { PX4Scheduler *sched = (PX4Scheduler *)arg; uint32_t last_ran_overtime = 0; pthread_setname_np(pthread_self(), "apm_timer"); while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } while (!_px4_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 ((PX4RCInput *)hal.rcin)->_timer_tick(); if (px4_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 *PX4Scheduler::_io_thread(void *arg) { PX4Scheduler *sched = (PX4Scheduler *)arg; while (!sched->_hal_initialized) { poll(NULL, 0, 1); } while (!_px4_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 *PX4Scheduler::_storage_thread(void *arg) { PX4Scheduler *sched = (PX4Scheduler *)arg; pthread_setname_np(pthread_self(), "apm_storage"); while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } while (!_px4_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 *PX4Scheduler::_io_thread(void *arg) { PX4Scheduler *sched = (PX4Scheduler *)arg; pthread_setname_np(pthread_self(), "apm_io"); while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } while (!_px4_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 *PX4Scheduler::_uart_thread(void *arg) { PX4Scheduler *sched = (PX4Scheduler *)arg; while (!sched->_hal_initialized) { poll(NULL, 0, 1); } while (!_px4_thread_should_exit) { sched->delay_microseconds_semaphore(1000); // process any pending serial bytes ((PX4UARTDriver *)hal.uartA)->_timer_tick(); ((PX4UARTDriver *)hal.uartB)->_timer_tick(); ((PX4UARTDriver *)hal.uartC)->_timer_tick(); ((PX4UARTDriver *)hal.uartD)->_timer_tick(); ((PX4UARTDriver *)hal.uartE)->_timer_tick(); } return NULL; }
void *PX4Scheduler::_uart_thread(void *arg) { PX4Scheduler *sched = (PX4Scheduler *)arg; pthread_setname_np(pthread_self(), "apm_uart"); while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } while (!_px4_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) { hal.uartA->begin(115200); hal.uartB->begin(38400); hal.uartC->begin(57600); hal.uartD->begin(57600); hal.uartE->begin(57600); hal.scheduler->init(); hal.rcin->init(); hal.rcout->init(); hal.analogin->init(); 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 */ hal_px4_set_priority(APM_STARTUP_PRIORITY); schedulerInstance.hal_initialized(); g_callbacks->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 */ hal_px4_set_priority(APM_MAIN_PRIORITY); while (!_px4_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); g_callbacks->loop(); if (px4_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. */ hal_px4_set_priority(APM_MAIN_PRIORITY); perf_count(perf_overrun); px4_ran_overtime = false; } perf_end(perf_loop); /* give up 250 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(250); } thread_running = false; return 0; }