ArduPilot之开源代码Task介绍

文章详细介绍了ArduPilot飞控系统的任务结构,包括Copter飞控任务的数组定义,任务宏如SCHED_TASK_CLASS和FAST_TASK的使用,以及Functor类在任务调度中的作用。这些任务按照优先级执行,不被传统OS调度器中断,确保飞控任务完整执行。文章还探讨了基础知识如Functor的构造和回调操作,以及如何通过宏定义将类方法转换为任务函数。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

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<void,> 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 RetType, class... Args>
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<RetType, Args...>& rhs)
    {
        return _obj == rhs._obj && _method == rhs._method;
    }
    inline bool operator!=(const Functor<RetType, Args...>& 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<class T, RetType (T::*method)(Args...)>
    static constexpr Functor bind(T *obj)
    {
        return { obj, method_wrapper<T, method> };
    }

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

    template<class T, RetType (T::*method)(Args...)>
    static RetType method_wrapper(void *obj, Args... args)
    {
        T *t = static_cast<T*>(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<rettype, ## __VA_ARGS__> name

#define FUNCTOR_BIND(obj, func, rettype, ...) \
    Functor<rettype, ## __VA_ARGS__>::bind<std::remove_reference<decltype(*obj)>::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飞控启动&运行过程简介

### ArduPilot线程模型与工作原理 ArduPilot 是一个开源的自动驾驶仪软件,其设计目标是为无人机和其他机器人系统提供稳定、高效的控制能力。在多任务处理环境中,ArduPilot 使用了多种机制来确保线程之间的数据一致性和并发安全性。 #### 1. 线程模型概述 ArduPilot 的线程模型基于实时操作系统(RTOS),如 ChibiOS 或 NuttX。这些操作系统提供了多线程支持,允许不同的任务并行运行[^3]。每个线程通常负责特定的功能模块,例如传感器数据采集、导航计算、控制律执行等。通过合理分配线程优先级和时间片,ArduPilot 能够高效地管理多个任务。 #### 2. 数据共享与同步 当多个线程需要访问或修改同一份数据时,必须采取措施防止数据竞争和损坏。ArduPilot 提供了以下三种主要方法来解决这一问题: - **信号量**:信号量是一种经典的同步工具,用于协调线程间的访问顺序。通过获取和释放信号量,线程可以安全地访问共享资源[^3]。 - **无锁数据结构**:无锁数据结构利用原子操作来避免显式的锁定机制,从而提高性能并减少死锁风险。ArduPilot 在某些场景中采用了这种技术以优化关键路径上的效率[^3]。 - **PX4 ORB**:虽然 PX4 ORB 主要用于 PX4 自动驾驶仪框架,但 ArduPilot 也可以借鉴类似的思想来实现高效的消息传递和数据共享[^3]。 #### 3. 典型线程及其功能 以下是 ArduPilot 中一些常见的线程及其职责: - **传感器读取线程**:负责从惯性测量单元(IMU)、气压计、GPS 等传感器读取数据,并将结果存储到全局变量或缓冲区中[^1]。 - **控制律计算线程**:根据当前状态估计值和设定的目标值,计算所需的控制输入(如电机转速)。此线程通常具有较高的优先级以保证实时性[^2]。 - **通信接口线程**:处理与地面站或其他外部设备之间的通信任务,包括解析命令包、发送遥测数据等。 - **任务管理线程**:执行高级别的逻辑决策,例如自动起飞、着陆、返航以及障碍物规避等[^2]。 #### 4. 实现示例 以下是一个简单的代码片段,展示了如何使用信号量保护共享数据: ```c++ SemaphoreHandle_t xSemaphore; void initialize_semaphore() { xSemaphore = xSemaphoreCreateMutex(); } void thread_a_task(void *pvParameters) { if (xSemaphoreTake(xSemaphore, portMAX_DELAY)) { // 修改共享数据 shared_data.value++; xSemaphoreGive(xSemaphore); } } void thread_b_task(void *pvParameters) { if (xSemaphoreTake(xSemaphore, portMAX_DELAY)) { // 读取共享数据 int value = shared_data.value; xSemaphoreGive(xSemaphore); } } ``` ### 结论 ArduPilot 的线程模型结合了实时操作系统提供的多线程能力和多种同步机制,确保了复杂任务环境下的稳定性和效率。开发者可以根据具体需求选择合适的同步方式,并注意合理配置线程优先级以满足实时性要求。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值