ArduPilot开源代码之AP_Scheduler

1. 源由

AP_Scheduler是ArduPilot任务调度的核心，并且飞控的调度器都是目的性很强的自研调度算法。

因此，要理解飞控的应用，尤其是飞行姿态时间相关性极高的任务更需要理解调度算法逻辑。

2. AP_Scheduler类

AP_Scheduler类定义详见：libraries/AP_Scheduler/AP_Scheduler.h

系统启动时调用scheduler.init()进行初始化，然后再scheduler.loop()中循环，并通过scheduler.tick()记录标记心跳信息。

注：这里其实这里就是一个典型的bare metal循环，这种所谓的任务调度和OS中的任务概念是不一样的。但是，毕竟ArduPilot还是使用了ChibiOS这样的系统，那么肯定会存在一些数据有效性的问题，尤其是传感器获取的物理世界信息。

/*
  A task scheduler for APM main loops

  Sketches should call scheduler.init() on startup, then call
  scheduler.tick() at regular intervals (typically every 10ms).

  To run tasks use scheduler.run(), passing the amount of time that
  the scheduler is allowed to use before it must return
 */

class AP_Scheduler
{
public:
    AP_Scheduler();

    /* Do not allow copies */
    CLASS_NO_COPY(AP_Scheduler);

    static AP_Scheduler *get_singleton();
    static AP_Scheduler *_singleton;

    FUNCTOR_TYPEDEF(task_fn_t, void);

    struct Task {
        task_fn_t function;
        const char *name;
        float rate_hz;
        uint16_t max_time_micros;
        uint8_t priority; // task priority
    };

    enum class Options : uint8_t {
        RECORD_TASK_INFO = 1 << 0
    };

    enum FastTaskPriorities {
        FAST_TASK_PRI0 = 0,
        FAST_TASK_PRI1 = 1,
        FAST_TASK_PRI2 = 2,
        MAX_FAST_TASK_PRIORITIES = 3
    };

    // initialise scheduler
    void init(const Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit);

    // called by vehicle's main loop - which should be the only thing
    // that function does
    void loop();

    // call to update any logging the scheduler might do; call at 1Hz
    void update_logging();

    // write out PERF message to logger
    void Log_Write_Performance();

    // call when one tick has passed
    void tick(void);

    // return current tick counter
    uint16_t ticks() const { return _tick_counter; }
    uint32_t ticks32() const { return _tick_counter32; }

    // run the tasks. Call this once per 'tick'.
    // time_available is the amount of time available to run
    // tasks in microseconds
    void run(uint32_t time_available);

    // return the number of microseconds available for the current task
    uint16_t time_available_usec(void) const;

    // return debug parameter
    uint8_t debug_flags(void) { return _debug; }

    // return load average, as a number between 0 and 1. 1 means
    // 100% load. Calculated from how much spare time we have at the
    // end of a run()
    float load_average();

    // get the active main loop rate
    uint16_t get_loop_rate_hz(void) {
        if (_active_loop_rate_hz == 0) {
            _active_loop_rate_hz = _loop_rate_hz;
        }
        return _active_loop_rate_hz;
    }
    // get the time-allowed-per-loop in microseconds
    uint32_t get_loop_period_us() {
        if (_loop_period_us == 0) {
            _loop_period_us = 1000000UL / _loop_rate_hz;
        }
        return _loop_period_us;
    }
    // get the time-allowed-per-loop in seconds
    float get_loop_period_s() {
        if (is_zero(_loop_period_s)) {
            _loop_period_s = 1.0f / _loop_rate_hz;
        }
        return _loop_period_s;
    }

    // get the filtered main loop time in seconds
    float get_filtered_loop_time(void) const {
        return perf_info.get_filtered_time();
    }

    // get the filtered active main loop rate
    float get_filtered_loop_rate_hz() {
        return perf_info.get_filtered_loop_rate_hz();
    }

    // get the time in seconds that the last loop took
    float get_last_loop_time_s(void) const {
        return _last_loop_time_s;
    }

    // get the amount of extra time being added on each loop
    uint32_t get_extra_loop_us(void) const {
        return extra_loop_us;
    }

    HAL_Semaphore &get_semaphore(void) { return _rsem; }

    void task_info(ExpandingString &str);

    static const struct AP_Param::GroupInfo var_info[];

    // loop performance monitoring:
    AP::PerfInfo perf_info;

private:
    // used to enable scheduler debugging
    AP_Int8 _debug;

    // overall scheduling rate in Hz
    AP_Int16 _loop_rate_hz;

    // loop rate in Hz as set at startup
    AP_Int16 _active_loop_rate_hz;

    // scheduler options
    AP_Int8 _options;
    
    // calculated loop period in usec
    uint16_t _loop_period_us;

    // calculated loop period in seconds
    float _loop_period_s;
    
    // list of tasks to run
    const struct Task *_vehicle_tasks;
    uint8_t _num_vehicle_tasks;

    // list of common tasks to run
    const struct Task *_common_tasks;
    uint8_t _num_common_tasks;

    // total number of tasks in _tasks and _common_tasks list
    uint8_t _num_tasks;

    // number of 'ticks' that have passed (number of times that
    // tick() has been called
    uint16_t _tick_counter;
    uint32_t _tick_counter32;

    // tick counter at the time we last ran each task
    uint16_t *_last_run;

    // number of microseconds allowed for the current task
    uint32_t _task_time_allowed;

    // the time in microseconds when the task started
    uint32_t _task_time_started;

    // number of spare microseconds accumulated
    uint32_t _spare_micros;

    // number of ticks that _spare_micros is counted over
    uint8_t _spare_ticks;

    // start of loop timing
    uint32_t _loop_timer_start_us;

    // time of last loop in seconds
    float _last_loop_time_s;
    
    // bitmask bit which indicates if we should log PERF message
    uint32_t _log_performance_bit;

    // maximum task slowdown compared to desired task rate before we
    // start giving extra time per loop
    const uint8_t max_task_slowdown = 4;

    // counters to handle dynamically adjusting extra loop time to
    // cope with low CPU conditions
    uint32_t task_not_achieved;
    uint32_t task_all_achieved;
    
    // extra time available for each loop - used to dynamically adjust
    // the loop rate in case we are well over budget
    uint32_t extra_loop_us;


    // semaphore that is held while not waiting for ins samples
    HAL_Semaphore _rsem;
};

3. AP_Scheduler主要方法

3.1 AP_Scheduler构造

构造函数，主要对_singleton赋值以及AP_Scheduler的默认参数设置。

注：默认copter的调度器频率应该是400Hz，如果移植代码，没有在这个频率就要想想实时性的问题。

AP_Scheduler::AP_Scheduler()
{
    if (_singleton) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_HAL::panic("Too many schedulers");
#endif
        return;
    }
    _singleton = this;

    AP_Param::setup_object_defaults(this, var_info);
}

const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
    // @Param: DEBUG
    // @DisplayName: Scheduler debug level
    // @Description: Set to non-zero to enable scheduler debug messages. When set to show "Slips" the scheduler will display a message whenever a scheduled task is delayed due to too much CPU load. When set to ShowOverruns the scheduled will display a message whenever a task takes longer than the limit promised in the task table.
    // @Values: 0:Disabled,2:ShowSlips,3:ShowOverruns
    // @User: Advanced
    AP_GROUPINFO("DEBUG",    0, AP_Scheduler, _debug, 0),

    // @Param: LOOP_RATE
    // @DisplayName: Scheduling main loop rate
    // @Description: This controls the rate of the main control loop in Hz. This should only be changed by developers. This only takes effect on restart. Values over 400 are considered highly experimental.
    // @Values: 50:50Hz,100:100Hz,200:200Hz,250:250Hz,300:300Hz,400:400Hz
    // @RebootRequired: True
    // @User: Advanced
    AP_GROUPINFO("LOOP_RATE",  1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE),

    // @Param: OPTIONS
    // @DisplayName: Scheduling options
    // @Description: This controls optional aspects of the scheduler.
    // @Bitmask: 0:Enable per-task perf info
    // @User: Advanced
    AP_GROUPINFO("OPTIONS",  2, AP_Scheduler, _options, 0),

    AP_GROUPEND
};

// declare a group var_info line
#define AP_GROUPINFO(name, idx, clazz, element, def) AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, 0)

#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduSub)
#define SCHEDULER_DEFAULT_LOOP_RATE 400
#else
#define SCHEDULER_DEFAULT_LOOP_RATE  50
#endif

3.2 init初始化

初始化任务调度类

1. 检查_loop_rate_hz范围[50, 2000]Hz //Copter 400Hz频率在这个范围之内；
2. 初始化设备相关任务_vehicle_tasks和_num_vehicle_tasks；
3. 初始化AP_Vehicle类_common_tasks和_num_common_tasks；
4. 初始化性能计数器和相应统计结构体；
5. sanity检查通用任务和_common_tasks和_vehicle_tasks优先级顺序；

void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit)
{
    // grab the semaphore before we start anything
    _rsem.take_blocking();

    // only allow 50 to 2000 Hz
    if (_loop_rate_hz < 50) {
        _loop_rate_hz.set(50);
    } else if (_loop_rate_hz > 2000) {
        _loop_rate_hz.set(2000);
    }
    _last_loop_time_s = 1.0 / _loop_rate_hz;

    _vehicle_tasks = tasks;
    _num_vehicle_tasks = num_tasks;

    AP_Vehicle* vehicle = AP::vehicle();
    if (vehicle != nullptr) {
        vehicle->get_common_scheduler_tasks(_common_tasks, _num_common_tasks);
    }

    _num_tasks = _num_vehicle_tasks + _num_common_tasks;

   _last_run = new uint16_t[_num_tasks];
    _tick_counter = 0;

    // setup initial performance counters
    perf_info.set_loop_rate(get_loop_rate_hz());
    perf_info.reset();

    if (_options & uint8_t(Options::RECORD_TASK_INFO)) {
        perf_info.allocate_task_info(_num_tasks);
    }

    _log_performance_bit = log_performance_bit;

    // sanity check the task lists to ensure the priorities are
    // never decrease
    uint8_t old = 0;
    for (uint8_t i=0; i<_num_common_tasks; i++) {
        if (_common_tasks[i].priority < old){
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            break;
        }
        old = _common_tasks[i].priority;
    }
    old = 0;
    for (uint8_t i=0; i<_num_vehicle_tasks; i++) {
        if (_vehicle_tasks[i].priority < old) {
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            break;
        }
        old = _vehicle_tasks[i].priority;
    }
}

3.3 loop循环

ArduPilot任务调度器最外层loop循环，整个循环的步骤：

1. 等待Gyro&Acc有效数据；
2. 更新系统tick，然后执行任务；
3. 性能数据统计，辅助参数更新；

void AP_Scheduler::loop()
{
    // wait for an INS sample
    hal.util->persistent_data.scheduler_task = -3;
    _rsem.give();
    AP::ins().wait_for_sample();  //等待Gyro&Acc有效采样数据
    _rsem.take_blocking();
    hal.util->persistent_data.scheduler_task = -1;

    const uint32_t sample_time_us = AP_HAL::micros();
    
    if (_loop_timer_start_us == 0) { //第一次进入loop循环
        _loop_timer_start_us = sample_time_us;
        _last_loop_time_s = get_loop_period_s(); //初始化默认调用时间间隔
    } else {
        _last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6; //计算每次实际loop调用时间间隔
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL  //SITL环境使用，通常实际硬件飞控不走这个流程。
    {
        /*
          for testing low CPU conditions we can add an optional delay in SITL
        */
        auto *sitl = AP::sitl();
        uint32_t loop_delay_us = sitl->loop_delay.get();
        hal.scheduler->delay_microseconds(loop_delay_us);
    }
#endif

    // tell the scheduler one tick has passed
    tick();

    // run all the tasks that are due to run. Note that we only
    // have to call this once per loop, as the tasks are scheduled
    // in multiples of the main loop tick. So if they don't run on
    // the first call to the scheduler they won't run on a later
    // call until scheduler.tick() is called again
    const uint32_t loop_us = get_loop_period_us();
    uint32_t now = AP_HAL::micros();
    uint32_t time_available = 0;
    const uint32_t loop_tick_us = now - sample_time_us;
    if (loop_tick_us < loop_us) {
        // get remaining time available for this loop
        time_available = loop_us - loop_tick_us;
    }

    // add in extra loop time determined by not achieving scheduler tasks
    time_available += extra_loop_us;

    // run the tasks
    run(time_available);  //使用剩余时间执行其他任务

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    // move result of AP_HAL::micros() forward:
    hal.scheduler->delay_microseconds(1);
#endif

    if (task_not_achieved > 0) {
        // add some extra time to the budget
        extra_loop_us = MIN(extra_loop_us+100U, 5000U);
        task_not_achieved = 0;
        task_all_achieved = 0;
    } else if (extra_loop_us > 0) {
        task_all_achieved++;
        if (task_all_achieved > 50) {
            // we have gone through 50 loops without a task taking too
            // long. CPU pressure has eased, so drop the extra time we're
            // giving each loop
            task_all_achieved = 0;
            // we are achieving all tasks, slowly lower the extra loop time
            extra_loop_us = MAX(0U, extra_loop_us-50U);
        }
    }

    // check loop time
    perf_info.check_loop_time(sample_time_us - _loop_timer_start_us); 
        
    _loop_timer_start_us = sample_time_us;

#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL  //HITL环境下，更新模拟外部数据
    hal.simstate->update();
#endif
}

3.4 run执行任务

默认情况下，Copter 400Hz频率进行loop，并在每次loop过程检查所有任务，判断是否执行。

1. 根据优先级判断vehicle_task和common_task哪个任务先行执行；
2. 快速任务和非快速任务在有限剩余时长下，决定是否执行；
3. 执行任务；
4. 统计任务执行时长，并进行辅助参数更新；

/*
  run one tick
  this will run as many scheduler tasks as we can in the specified time
 */
void AP_Scheduler::run(uint32_t time_available)
{
    uint32_t run_started_usec = AP_HAL::micros();
    uint32_t now = run_started_usec;

    uint8_t vehicle_tasks_offset = 0;  //ArduPilot copter代码有两部分组成：common_tasks和vehicle_tasks。
    uint8_t common_tasks_offset = 0;

    //决定哪种任务先开始执行
    for (uint8_t i=0; i<_num_tasks; i++) {  
        // determine which of the common task / vehicle task to run
        bool run_vehicle_task = false;
        if (vehicle_tasks_offset < _num_vehicle_tasks &&
            common_tasks_offset < _num_common_tasks) {
            // still have entries on both lists; compare the
            // priorities.  In case of a tie the vehicle-specific
            // entry wins.
            const Task &vehicle_task = _vehicle_tasks[vehicle_tasks_offset];
            const Task &common_task = _common_tasks[common_tasks_offset];
            if (vehicle_task.priority <= common_task.priority) {
                run_vehicle_task = true;
            }
        } else if (vehicle_tasks_offset < _num_vehicle_tasks) {
            // out of common tasks to run
            run_vehicle_task = true;
        } else if (common_tasks_offset < _num_common_tasks) {
            // out of vehicle tasks to run
            run_vehicle_task = false;
        } else {
            // this is an error; the outside loop should have terminated
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            break;
        }

        const AP_Scheduler::Task &task = run_vehicle_task ? _vehicle_tasks[vehicle_tasks_offset] : _common_tasks[common_tasks_offset];
        if (run_vehicle_task) {
            vehicle_tasks_offset++;
        } else {
            common_tasks_offset++;
        }

        if (task.priority > MAX_FAST_TASK_PRIORITIES) {  //非快速任务
            const uint16_t dt = _tick_counter - _last_run[i];
            // we allow 0 to mean loop rate
            uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz);
            if (interval_ticks < 1) {
                interval_ticks = 1;
            }
            if (dt < interval_ticks) {  //尚未达到执行周期
                // this task is not yet scheduled to run again
                continue;
            }
            // this task is due to run. Do we have enough time to run it?
            _task_time_allowed = task.max_time_micros;

            if (dt >= interval_ticks*2) {  //错过一次
                perf_info.task_slipped(i);
            }

            if (dt >= interval_ticks*max_task_slowdown) {  //严重超时
                // we are going beyond the maximum slowdown factor for a
                // task. This will trigger increasing the time budget
                task_not_achieved++;
            }

            if (_task_time_allowed > time_available) {  //剩余时间不够运行该非快速任务，需要下一个周期检查
                // not enough time to run this task.  Continue loop -
                // maybe another task will fit into time remaining
                continue;
            }
        } else {  //快速任务，必须再每个周期得到执行
            _task_time_allowed = get_loop_period_us();
        }

        // run it
        _task_time_started = now;
        hal.util->persistent_data.scheduler_task = i;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        fill_nanf_stack();
#endif
        task.function();
        hal.util->persistent_data.scheduler_task = -1;

        // record the tick counter when we ran. This drives
        // when we next run the event
        _last_run[i] = _tick_counter;

        // work out how long the event actually took
        now = AP_HAL::micros();
        uint32_t time_taken = now - _task_time_started;  //本次任务执行时长
        bool overrun = false;
        if (time_taken > _task_time_allowed) {  //运行时间超过允许时长
            overrun = true;
            // the event overran!
            debug(3, "Scheduler overrun task[%u-%s] (%u/%u)\n",
                  (unsigned)i,
                  task.name,
                  (unsigned)time_taken,
                  (unsigned)_task_time_allowed);
        }

        perf_info.update_task_info(i, time_taken, overrun);

        //更新剩余时间
        if (time_taken >= time_available) { 
            /*
              we are out of time, but we need to keep walking the task
              table in case there is another fast loop task after this
              task, plus we need to update the accouting so we can
              work out if we need to allocate extra time for the loop
              (lower the loop rate)
              Just set time_available to zero, which means we will
              only run fast tasks after this one
             */
            time_available = 0;
        } else {
            time_available -= time_taken;
        }
    }

    // update number of spare microseconds
    _spare_micros += time_available;

    _spare_ticks++;
    if (_spare_ticks == 32) {
        _spare_ticks /= 2;
        _spare_micros /= 2;
    }
}

4. AP_Scheduler其他方法

4.1 load_average占用率

任务执行占用CPU的时间。

/*
  calculate load average as a number from 0 to 1
 */
float AP_Scheduler::load_average()
{
    // return 1 if filtered main loop rate is 5% below the configured rate
    if (get_filtered_loop_rate_hz() < get_loop_rate_hz() * 0.95) {  //去噪（滤波）后的循环时间，如果超过95%循环间隔，那就是100%CPU
        return 1.0;
    }
    if (_spare_ticks == 0) {  //loop循环在跑就不太可能是0, 这个场景基本不存在
        return 0.0f;
    }
    const uint32_t loop_us = get_loop_period_us();
    const uint32_t used_time = loop_us - (_spare_micros/_spare_ticks);
    return constrain_float(used_time / (float)loop_us, 0, 1);  //剩余时间比上单位时间 = 任务占用的CPU百分率
}

4.2 task_info任务信息

1. task: 任务名
2. MIN: 最小执行时间
3. MAX: 最大执行时间
4. AVG: 平均执行时间
5. OVR: 超时次数
6. SLP: 错过次数
7. TOT: 占总任务的百分比

// display task statistics as text buffer for @SYS/tasks.txt
void AP_Scheduler::task_info(ExpandingString &str)
{
    // a header to allow for machine parsers to determine format
    str.printf("TasksV2\n");

    // dynamically enable statistics collection
    if (!(_options & uint8_t(Options::RECORD_TASK_INFO))) {
        _options.set(_options | uint8_t(Options::RECORD_TASK_INFO));
        return;
    }

    if (perf_info.get_task_info(0) == nullptr) {
        return;
    }

    // baseline the total time taken by all tasks
    float total_time = 1.0f;
    for (uint8_t i = 0; i < _num_tasks + 1; i++) {
        const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i);
        if (ti != nullptr && ti->tick_count > 0) {
            total_time += ti->elapsed_time_us;
        }
    }

    uint8_t vehicle_tasks_offset = 0;
    uint8_t common_tasks_offset = 0;

    for (uint8_t i = 0; i < _num_tasks; i++) {
        const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i);
        const char *task_name;

        // determine which of the common task / vehicle task to run
        bool run_vehicle_task = false;
        if (vehicle_tasks_offset < _num_vehicle_tasks &&
            common_tasks_offset < _num_common_tasks) {
            // still have entries on both lists; compare the
            // priorities.  In case of a tie the vehicle-specific
            // entry wins.
            const Task &vehicle_task = _vehicle_tasks[vehicle_tasks_offset];
            const Task &common_task = _common_tasks[common_tasks_offset];
            if (vehicle_task.priority <= common_task.priority) {
                run_vehicle_task = true;
            }
        } else if (vehicle_tasks_offset < _num_vehicle_tasks) {
            // out of common tasks to run
            run_vehicle_task = true;
        } else if (common_tasks_offset < _num_common_tasks) {
            // out of vehicle tasks to run
            run_vehicle_task = false;
        } else {
            // this is an error; the outside loop should have terminated
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            return;
        }

        if (run_vehicle_task) {
            task_name = _vehicle_tasks[vehicle_tasks_offset++].name;
        } else {
            task_name = _common_tasks[common_tasks_offset++].name;
        }

        ti->print(task_name, total_time, str);
    }
}

5. 总结

ArduPilot的AP_Scheduler，整体上看，代码还是比较清晰的，且比较容易定位以下情况：

1. 超时
2. 饿死
3. 高占用率

6. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot飞控之ubuntu22.04-SITL安装
【4】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【5】ArduPilot飞控之Mission Planner模拟
【6】ArduPilot飞控AOCODARC-H7DUAL固件编译
【7】ArduPilot之开源代码Library&Sketches设计
【8】ArduPilot之开源代码Sensor Drivers设计
【9】ArduPilot之开源代码基础知识&Threading概念
【10】ArduPilot之开源代码UARTs and the Console使用
【11】ArduPilot飞控启动&运行过程简介
【11】ArduPilot之开源代码Task介绍
【12】ArduPilot开源代码之AP_Param