C++ PX4Scheduler类代码示例

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

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