ArduPilot之开源代码Task介绍

1. 源由

在ArduPilot飞控启动&运行过程简介中，从C语言main函数入口，到最终ArduPilot自研调度器开始执行任务，整个串联起来。

真正的飞控应用是在ArduPilot任务中动态实现的，要注意ArduPilot任务和传统的OS调度器调度的任务是不太一样的，传统OS调度任务可以再任务执行的任何时刻被切换出去，但是飞控任务有其特殊性，每次任务执行都将完整的执行完一轮。

那么，到底有哪些任务，怎么分类任务，飞控应用任务的归类是如何来理解的呢？

接下来我们就将会针对ArduPilot任务展开分析和研讨。

2. Copter飞控任务

注：鉴于Copter设计文档资料是最全的，因此我们这里也以Copter飞控进行研读。这样至少可以参考的设计资料可以更加多一些，方便我们理解和体会设计的思路。

ArduCopter/Copter.cppconst AP_Scheduler::Task Copter::scheduler_tasks[]数组包含了所用运行时的任务。

/*
  scheduler table - all tasks should be listed here.

  All entries in this table must be ordered by priority.

  This table is interleaved with the table in AP_Vehicle to determine
  the order in which tasks are run.  Convenience methods SCHED_TASK
  and SCHED_TASK_CLASS are provided to build entries in this structure:

SCHED_TASK arguments:
 - name of static function to call
 - rate (in Hertz) at which the function should be called
 - expected time (in MicroSeconds) that the function should take to run
 - priority (0 through 255, lower number meaning higher priority)

SCHED_TASK_CLASS arguments:
 - class name of method to be called
 - instance on which to call the method
 - method to call on that instance
 - rate (in Hertz) at which the method should be called
 - expected time (in MicroSeconds) that the method should take to run
 - priority (0 through 255, lower number meaning higher priority)

 */
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    // update INS immediately to get current gyro data populated
    FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
    // run low level rate controllers that only require IMU data
    FAST_TASK(run_rate_controller),
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
    FAST_TASK(run_custom_controller),
#endif
#if FRAME_CONFIG == HELI_FRAME
    FAST_TASK(heli_update_autorotation),
#endif //HELI_FRAME
    // send outputs to the motors library immediately
    FAST_TASK(motors_output),
     // run EKF state estimator (expensive)
    FAST_TASK(read_AHRS),
#if FRAME_CONFIG == HELI_FRAME
    FAST_TASK(update_heli_control_dynamics),
#endif //HELI_FRAME
    // Inertial Nav
    FAST_TASK(read_inertia),
    // check if ekf has reset target heading or position
    FAST_TASK(check_ekf_reset),
    // run the attitude controllers
    FAST_TASK(update_flight_mode),
    // update home from EKF if necessary
    FAST_TASK(update_home_from_EKF),
    // check if we've landed or crashed
    FAST_TASK(update_land_and_crash_detectors),
    // surface tracking update
    FAST_TASK(update_rangefinder_terrain_offset),
#if HAL_MOUNT_ENABLED
    // camera mount's fast update
    FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
#endif
    FAST_TASK(Log_Video_Stabilisation),

    SCHED_TASK(rc_loop,              250,    130,  3),
    SCHED_TASK(throttle_loop,         50,     75,  6),
    SCHED_TASK_CLASS(AP_GPS,               &copter.gps,                 update,          50, 200,   9),
#if AP_OPTICALFLOW_ENABLED
    SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),
#endif
    SCHED_TASK(update_batt_compass,   10,    120, 15),
    SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all,    10,  50,  18),
    SCHED_TASK(arm_motors_check,      10,     50, 21),
#if TOY_MODE_ENABLED == ENABLED
    SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),
#endif
    SCHED_TASK(auto_disarm_check,     10,     50,  27),
    SCHED_TASK(auto_trim,             10,     75,  30),
#if RANGEFINDER_ENABLED == ENABLED
    SCHED_TASK(read_rangefinder,      20,    100,  33),
#endif
#if HAL_PROXIMITY_ENABLED
    SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50,  36),
#endif
#if BEACON_ENABLED == ENABLED
    SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50,  39),
#endif
    SCHED_TASK(update_altitude,       10,    100,  42),
    SCHED_TASK(run_nav_updates,       50,    100,  45),
    SCHED_TASK(update_throttle_hover,100,     90,  48),
#if MODE_SMARTRTL_ENABLED == ENABLED
    SCHED_TASK_CLASS(ModeSmartRTL,         &copter.mode_smartrtl,       save_position,    3, 100,  51),
#endif
#if HAL_SPRAYER_ENABLED
    SCHED_TASK_CLASS(AC_Sprayer,           &copter.sprayer,               update,         3,  90,  54),
#endif
    SCHED_TASK(three_hz_loop,          3,     75, 57),
    SCHED_TASK_CLASS(AP_ServoRelayEvents,  &copter.ServoRelayEvents,      update_events, 50,  75,  60),
    SCHED_TASK_CLASS(AP_Baro,              &copter.barometer,             accumulate,    50,  90,  63),
#if PRECISION_LANDING == ENABLED
    SCHED_TASK(update_precland,      400,     50,  69),
#endif
#if FRAME_CONFIG == HELI_FRAME
    SCHED_TASK(check_dynamic_flight,  50,     75,  72),
#endif
#if LOGGING_ENABLED == ENABLED
    SCHED_TASK(loop_rate_logging, LOOP_RATE,    50,  75),
#endif
    SCHED_TASK_CLASS(AP_Notify,            &copter.notify,              update,          50,  90,  78),
    SCHED_TASK(one_hz_loop,            1,    100,  81),
    SCHED_TASK(ekf_check,             10,     75,  84),
    SCHED_TASK(check_vibration,       10,     50,  87),
    SCHED_TASK(gpsglitch_check,       10,     50,  90),
    SCHED_TASK(takeoff_check,         50,     50,  91),
#if AP_LANDINGGEAR_ENABLED
    SCHED_TASK(landinggear_update,    10,     75,  93),
#endif
    SCHED_TASK(standby_update,        100,    75,  96),
    SCHED_TASK(lost_vehicle_check,    10,     50,  99),
    SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
    SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550, 105),
#if HAL_MOUNT_ENABLED
    SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75, 108),
#endif
#if AP_CAMERA_ENABLED
    SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update,          50,  75, 111),
#endif
#if LOGGING_ENABLED == ENABLED
    SCHED_TASK(ten_hz_logging_loop,   10,    350, 114),
    SCHED_TASK(twentyfive_hz_logging, 25,    110, 117),
    SCHED_TASK_CLASS(AP_Logger,            &copter.logger,              periodic_tasks, 400, 300, 120),
#endif
    SCHED_TASK_CLASS(AP_InertialSensor,    &copter.ins,                 periodic,       400,  50, 123),

    SCHED_TASK_CLASS(AP_Scheduler,         &copter.scheduler,           update_logging, 0.1,  75, 126),
#if AP_RPM_ENABLED
    SCHED_TASK_CLASS(AP_RPM,               &copter.rpm_sensor,          update,          40, 200, 129),
#endif
    SCHED_TASK_CLASS(AP_TempCalibration,   &copter.g2.temp_calibration, update,          10, 100, 135),
#if HAL_ADSB_ENABLED
    SCHED_TASK(avoidance_adsb_update, 10,    100, 138),
#endif
#if ADVANCED_FAILSAFE == ENABLED
    SCHED_TASK(afs_fs_check,          10,    100, 141),
#endif
#if AP_TERRAIN_AVAILABLE
    SCHED_TASK(terrain_update,        10,    100, 144),
#endif
#if AP_GRIPPER_ENABLED
    SCHED_TASK_CLASS(AP_Gripper,           &copter.g2.gripper,          update,          10,  75, 147),
#endif
#if AP_WINCH_ENABLED
    SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50, 150),
#endif
#ifdef USERHOOK_FASTLOOP
    SCHED_TASK(userhook_FastLoop,    100,     75, 153),
#endif
#ifdef USERHOOK_50HZLOOP
    SCHED_TASK(userhook_50Hz,         50,     75, 156),
#endif
#ifdef USERHOOK_MEDIUMLOOP
    SCHED_TASK(userhook_MediumLoop,   10,     75, 159),
#endif
#ifdef USERHOOK_SLOWLOOP
    SCHED_TASK(userhook_SlowLoop,      3.3,   75, 162),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
    SCHED_TASK(userhook_SuperSlowLoop, 1,     75, 165),
#endif
#if HAL_BUTTON_ENABLED
    SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
#endif
#if STATS_ENABLED == ENABLED
    SCHED_TASK_CLASS(AP_Stats,             &copter.g2.stats,            update,           1, 100, 171),
#endif
};

3. ArduPilot任务宏定义

libraries/AP_Scheduler/AP_Scheduler.h给出的任务定义：

1. 任务函数：function
2. 任务名称：name
3. 任务频率：rate_hz
4. 最大耗时：max_time_micros
5. 任务优先级：priority

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

注：这里的task_fn_t function是一个对象。请参考3.1 基础知识

FUNCTOR_TYPEDEF(task_fn_t, void);
 └──> #define FUNCTOR_TYPEDEF(task_fn_t, void, ...) \
     └──> typedef Functor task_fn_t

4. 基础知识

4.1 Functor类

libraries/AP_HAL/utility/functor.h

• 构造函数：

constexpr Functor(void *obj, RetType (*method)(void *obj, Args…))
constexpr Functor(decltype(nullptr))
constexpr Functor()

• 回调操作：RetType operator()(Args… args) const

• 基本运算：=/!=/bool()

• 函数打包：将C++类的方法打包成C语言函数（作为任务函数）

Functor类

template 
class Functor
{
public:
    constexpr Functor(void *obj, RetType (*method)(void *obj, Args...))
        : _obj(obj)
        , _method(method)
    {
    }

    // Allow to construct an empty Functor
    constexpr Functor(decltype(nullptr))
        : Functor(nullptr, nullptr) { }

    constexpr Functor()
        : Functor(nullptr, nullptr) { }

    // Call the method on the obj this Functor is bound to
    RetType operator()(Args... args) const
    {
        return _method(_obj, args...);
    }

    // Compare if the two Functors are calling the same method in the same
    // object
    inline bool operator==(const Functor& rhs)
    {
        return _obj == rhs._obj && _method == rhs._method;
    }
    inline bool operator!=(const Functor& rhs)
    {
        return _obj != rhs._obj || _method != rhs._method;
    }

    // Allow to check if there's a method set in the Functor
    explicit operator bool() const
    {
        return _method != nullptr;
    }

    template
    static constexpr Functor bind(T *obj)
    {
        return { obj, method_wrapper };
    }

private:
    void *_obj;
    RetType (*_method)(void *obj, Args...);

    template
    static RetType method_wrapper(void *obj, Args... args)
    {
        T *t = static_cast(obj);
        return (t->*method)(args...);
    }
};

decltype
Inspects the declared type of an entity or the type and value category of an expression.

constexpr
The constexpr specifier declares that it is possible to evaluate the value of the function or variable at compile time.
Such variables and functions can then be used where only compile time constant expressions are allowed (provided that appropriate function arguments are given).

4.2 Functor相关宏定义

这里的宏定义大致有两个目的：

1. 将类函数塞进scheduler
2. 将C函数关联到tasks数组
3. 为了节省Flash空间将任务名字进行缩减

#define FUNCTOR_TYPEDEF(name, rettype, ...) \
    typedef Functor name

#define FUNCTOR_BIND(obj, func, rettype, ...) \
    Functor::bind::type, func>(obj)

#if HAL_MINIMIZE_FEATURES
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_name,
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_name "*",
#else
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name,
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name "*",
#endif

remove_reference
If the type T is a reference type, provides the member typedef type which is the type referred to by T. Otherwise type is T.

5. 任务声明

5.1 SCHED_TASK_CLASS

将其映射到任务结构

1. 任务函数：FUNCTOR_BIND(classptr, &classname::func, void)
2. 任务名称：AP_SCHEDULER_NAME_INITIALIZER(classname, func)
3. 任务频率：_rate_hz
4. 最大耗时：_max_time_micros
5. 任务优先级：_priority

#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros, _priority) { \
    .function = FUNCTOR_BIND(classptr, &classname::func, void),\
    AP_SCHEDULER_NAME_INITIALIZER(classname, func)\
    .rate_hz = _rate_hz,\
    .max_time_micros = _max_time_micros,        \
    .priority = _priority \
}

SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168),为例：

1. 任务函数：FUNCTOR_BIND(&copter.button, &AP_Button::update, void)
2. 任务名称：AP_SCHEDULER_NAME_INITIALIZER(AP_Button, update)
3. 任务频率：5
4. 最大耗时：100
5. 任务优先级：168

5.2 SCHED_TASK

#define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio)

SCHED_TASK(rc_loop, 250, 130, 3),为例：

1. 任务函数：FUNCTOR_BIND(&vehicle, &AP_Vehicle::rc_loop, void)
2. 任务名称：AP_SCHEDULER_NAME_INITIALIZER(AP_Vehicle, rc_loop)
3. 任务频率：250
4. 最大耗时：130
5. 任务优先级：3

注：这里为什么将指针转换到AP_Vehicle类，不是很明白。

5.3 FAST_TASK_CLASS

将其映射到任务结构

1. 任务函数：FUNCTOR_BIND(classptr, &classname::func, void)
2. 任务名称：AP_SCHEDULER_NAME_INITIALIZER(classname, func)
3. 任务频率：_rate_hz
4. 最大耗时：_max_time_micros
5. 任务优先级：_priority

#define FAST_TASK_CLASS(classname, classptr, func) { \
    .function = FUNCTOR_BIND(classptr, &classname::func, void),\
    AP_FAST_NAME_INITIALIZER(classname, func)\
    .rate_hz = 0,\
    .max_time_micros = 0,\
    .priority = AP_Scheduler::FAST_TASK_PRI0 \
}

FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),为例：

1. 任务函数：FUNCTOR_BIND(&copter.camera_mount, &AP_Mount::update_fast, void)
2. 任务名称：AP_SCHEDULER_NAME_INITIALIZER(AP_Mount, update_fast)
3. 任务频率：0
4. 最大耗时：0
5. 任务优先级：AP_Scheduler::FAST_TASK_PRI0

5.4 FAST_TASK

#define FAST_TASK(func) FAST_TASK_CLASS(Copter, &copter, func)

FAST_TASK(read_inertia),为例：

1. 任务函数：FUNCTOR_BIND(&copter, &Copter::read_inertia, void)
2. 任务名称：AP_SCHEDULER_NAME_INITIALIZER(Copter, read_inertia)
3. 任务频率：0
4. 最大耗时：0
5. 任务优先级：AP_Scheduler::FAST_TASK_PRI0

6. 总结

通过scheduler_tasks中定义的任务及其优先级，AP_Scheduler类将ArduPilot进行飞控业务的调度执行。总的来说，只有在了解大体任务切分（业务），随后就是业务的优先级以及调度内容。

关于具体业务调度的算法和逻辑，我们将在后续章节中展开。

7. 参考资料

【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飞控启动&运行过程简介