文章详细介绍了ArduPilot飞控系统的任务结构，包括Copter飞控任务的数组定义，任务宏如SCHED_TASK_CLASS和FAST_TASK的使用，以及Functor类在任务调度中的作用。这些任务按照优先级执行，不被传统OS调度器中断，确保飞控任务完整执行。文章还探讨了基础知识如Functor的构造和回调操作，以及如何通过宏定义将类方法转换为任务函数。
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给出的任务定义：
- 任务函数：function
- 任务名称：name
- 任务频率：rate_hz
- 最大耗时：max_time_micros
- 任务优先级：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相关宏定义
这里的宏定义大致有两个目的：
- 将类函数塞进scheduler
- 将C函数关联到tasks数组
- 为了节省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
将其映射到任务结构
- 任务函数：FUNCTOR_BIND(classptr, &classname::func, void)
- 任务名称：AP_SCHEDULER_NAME_INITIALIZER(classname, func)
- 任务频率：_rate_hz
- 最大耗时：_max_time_micros
- 任务优先级：_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),为例：
- 任务函数：FUNCTOR_BIND(&copter.button, &AP_Button::update, void)
- 任务名称：AP_SCHEDULER_NAME_INITIALIZER(AP_Button, update)
- 任务频率：5
- 最大耗时：100
- 任务优先级：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),为例：
- 任务函数：FUNCTOR_BIND(&vehicle, &AP_Vehicle::rc_loop, void)
- 任务名称：AP_SCHEDULER_NAME_INITIALIZER(AP_Vehicle, rc_loop)
- 任务频率：250
- 最大耗时：130
- 任务优先级：3
注：这里为什么将指针转换到AP_Vehicle类，不是很明白。
5.3 FAST_TASK_CLASS
将其映射到任务结构
- 任务函数：FUNCTOR_BIND(classptr, &classname::func, void)
- 任务名称：AP_SCHEDULER_NAME_INITIALIZER(classname, func)
- 任务频率：_rate_hz
- 最大耗时：_max_time_micros
- 任务优先级：_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),为例：
- 任务函数：FUNCTOR_BIND(&copter.camera_mount, &AP_Mount::update_fast, void)
- 任务名称：AP_SCHEDULER_NAME_INITIALIZER(AP_Mount, update_fast)
- 任务频率：0
- 最大耗时：0
- 任务优先级：AP_Scheduler::FAST_TASK_PRI0
5.4 FAST_TASK
#define FAST_TASK(func) FAST_TASK_CLASS(Copter, &copter, func)
FAST_TASK(read_inertia),为例：
- 任务函数：FUNCTOR_BIND(&copter, &Copter::read_inertia, void)
- 任务名称：AP_SCHEDULER_NAME_INITIALIZER(Copter, read_inertia)
- 任务频率：0
- 最大耗时：0
- 任务优先级：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飞控启动&运行过程简介
扫一扫
3118
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
