ArduPilot开源代码之AP_Scheduler

• 1. 源由
• 2. AP_Scheduler类
• 3. AP_Scheduler主要方法
• • 3.1 AP_Scheduler构造
  • 3.2 init初始化
  • 3.3 loop循环
  • 3.4 run执行任务
• 4. AP_Scheduler其他方法
• • 4.1 load_average占用率
  • 4.2 task_info任务信息
• 5. 总结
• 6. 参考资料

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 loopsSketches should call scheduler.init() on startup, then callscheduler.tick() at regular intervals (typically every 10ms).To run tasks use scheduler.run(), passing the amount of time thatthe 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 schedulervoid 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 doesvoid loop();// call to update any logging the scheduler might do; call at 1Hzvoid update_logging();// write out PERF message to loggervoid Log_Write_Performance();// call when one tick has passedvoid tick(void);// return current tick counteruint16_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 microsecondsvoid run(uint32_t time_available);// return the number of microseconds available for the current taskuint16_t time_available_usec(void) const;// return debug parameteruint8_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 rateuint16_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 microsecondsuint32_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 secondsfloat 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 secondsfloat get_filtered_loop_time(void) const {return perf_info.get_filtered_time();}// get the filtered active main loop ratefloat get_filtered_loop_rate_hz() {return perf_info.get_filtered_loop_rate_hz();}// get the time in seconds that the last loop tookfloat get_last_loop_time_s(void) const {return _last_loop_time_s;}// get the amount of extra time being added on each loopuint32_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 debuggingAP_Int8 _debug;// overall scheduling rate in HzAP_Int16 _loop_rate_hz;// loop rate in Hz as set at startupAP_Int16 _active_loop_rate_hz;// scheduler optionsAP_Int8 _options;// calculated loop period in usecuint16_t _loop_period_us;// calculated loop period in secondsfloat _loop_period_s;// list of tasks to runconst struct Task *_vehicle_tasks;uint8_t _num_vehicle_tasks;// list of common tasks to runconst struct Task *_common_tasks;uint8_t _num_common_tasks;// total number of tasks in _tasks and _common_tasks listuint8_t _num_tasks;// number of 'ticks' that have passed (number of times that// tick() has been calleduint16_t _tick_counter;uint32_t _tick_counter32;// tick counter at the time we last ran each taskuint16_t *_last_run;// number of microseconds allowed for the current taskuint32_t _task_time_allowed;// the time in microseconds when the task starteduint32_t _task_time_started;// number of spare microseconds accumulateduint32_t _spare_micros;// number of ticks that _spare_micros is counted overuint8_t _spare_ticks;// start of loop timinguint32_t _loop_timer_start_us;// time of last loop in secondsfloat _last_loop_time_s;// bitmask bit which indicates if we should log PERF messageuint32_t _log_performance_bit;// maximum task slowdown compared to desired task rate before we// start giving extra time per loopconst uint8_t max_task_slowdown = 4;// counters to handle dynamically adjusting extra loop time to// cope with low CPU conditionsuint32_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 budgetuint32_t extra_loop_us;// semaphore that is held while not waiting for ins samplesHAL_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_SITLAP_HAL::panic("Too many schedulers");
#endifreturn;}_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: AdvancedAP_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: AdvancedAP_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: AdvancedAP_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 Hzif (_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 countersperf_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 decreaseuint8_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 samplehal.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 passedtick();// 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 againconst 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 looptime_available = loop_us - loop_tick_us;}// add in extra loop time determined by not achieving scheduler taskstime_available += extra_loop_us;// run the tasksrun(time_available);  //使用剩余时间执行其他任务#if CONFIG_HAL_BOARD == HAL_BOARD_SITL// move result of AP_HAL::micros() forward:hal.scheduler->delay_microseconds(1);
#endifif (task_not_achieved > 0) {// add some extra time to the budgetextra_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 looptask_all_achieved = 0;// we are achieving all tasks, slowly lower the extra loop timeextra_loop_us = MAX(0U, extra_loop_us-50U);}}// check loop timeperf_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过程检查所有任务，判断是否执行。

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: AdvancedAP_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: AdvancedAP_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: AdvancedAP_GROUPINFO("OPTIONS",  2, AP_Scheduler, _options, 0),AP_GROUPEND
};

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

/*run one tickthis 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 runbool 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 runrun_vehicle_task = true;} else if (common_tasks_offset < _num_common_tasks) {// out of vehicle tasks to runrun_vehicle_task = false;} else {// this is an error; the outside loop should have terminatedINTERNAL_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 rateuint32_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 againcontinue;}// 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 budgettask_not_achieved++;}if (_task_time_allowed > time_available) {  //剩余时间不够运行该非快速任务，需要下一个周期检查// not enough time to run this task.  Continue loop -// maybe another task will fit into time remainingcontinue;}} 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_SITLfill_nanf_stack();
#endiftask.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 tooknow = 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 tasktable in case there is another fast loop task after thistask, plus we need to update the accouting so we canwork out if we need to allocate extra time for the loop(lower the loop rate)Just set time_available to zero, which means we willonly 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 rateif (get_filtered_loop_rate_hz() < get_loop_rate_hz() * 0.95) {  //去噪（滤波）后的循环时间，如果超过95%循环间隔，那就是100%CPUreturn 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 formatstr.printf("TasksV2\n");// dynamically enable statistics collectionif (!(_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 tasksfloat 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 runbool 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 runrun_vehicle_task = true;} else if (common_tasks_offset < _num_common_tasks) {// out of vehicle tasks to runrun_vehicle_task = false;} else {// this is an error; the outside loop should have terminatedINTERNAL_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

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！