ArduPilot之开源代码基础知识Threading概念
ArduPilot之开源代码基础知识&Threading概念
- 1. 源由
- 2. 基础知识
- 2.1 The timer callbacks
- 2.2 HAL specific threads
- 2.2.1 AP_HAL_ChibiOS
- 2.2.2 AP_HAL_Linux
- 2.2.3 AP_HAL_ESP32
- 2.3 driver specific threads
- 2.4 ardupilot drivers versus platform drivers
- 2.5 platform specific threads and tasks
- 2.6 the AP_Scheduler system
- 2.7 Resource Exclusion & Data Consistency
- 3. Threading概念
- 4. 参考资料
1. 源由
Ardunio的setup()/loop()设计概念比较适合独立功能级别的验证和测试。
相对于飞控复杂系统来说，就需要多线程/多任务的加持，为此，使用支持具有实时优先级的丰富的Posix线程模型就更为重要。
因此本章将就ArduPilot的基础知识和Threading概念做一个了解。
从AP_HAL硬件解耦设计看，ArduPilot有以下几种HAL分层：
- AP_HAL_ChibiOS
- AP_HAL_Linux
- AP_HAL_ESP32
- AP_HAL_Empty
- AP_HAL_SITL
注1：在ArduPilot之开源代码Library&Sketches设计中，已经介绍了飞控应用是如何通过AP_HAL_MAIN/AP_HAL_MAIN_CALLBACKS板级启动Library。
2. 基础知识
关于ArduPilot Threading概念之前，首先对以下概念回顾和澄清下：
2.1 The timer callbacks
每个平台在AP_HAL中提供一个1kHz的定时器。ArduPilot中的任何代码都可以注册一个定时器函数，然后以1kHz的频率调用该函数。
所有注册的定时器函数都是按顺序调用的。使用这种非常原始的机制是因为它非常方便有用。
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_update));
该特定示例来自MS5611气压计驱动程序。AP_HAL_MEMBERPROC()宏提供了一种将C++成员函数封装为回调参数的方法（将对象上下文与函数指针绑定在一起）。
当一段代码希望在低于1kHz的频率下发生某些事情时，它应该维护自己的“last_called”变量，如果时间不够，则立即返回。
可以使用hal.scheller->millis()和hal.scheduller->micros()函数来获取自启动以来的时间（以毫秒和微秒为单位）。
2.2 HAL specific threads
HAL是Hardware Abstration Layer的缩写，因此，重点关注AP_HAL_ChibiOS/AP_HAL_Linux/AP_HAL_ESP32板子对应HAL线程。
注：AP_HAL_Empty(裸奔 bear metal os)/AP_HAL_SITL(这个主要是Linux下模拟用，和硬件驱动关系不大了。
不同系统下HAL的启动过程大致如下：
hal.run
└──> main_loop
├──> hal.scheduler->init
├──> g_callbacks->setup
│ └──> AP_Vehicle::setup
└──> g_callbacks->loop
2.2.1 AP_HAL_ChibiOS
采用chThdCreateStatic建立以下线程：
- main_loop // 主应用线程
- _monitor_thread_wa
- _timer_thread_wa
- _rcout_thread_wa
- _rcin_thread_wa
- _io_thread_wa
- _storage_thread_wa
void Scheduler::init()
{
chBSemObjectInit(&_timer_semaphore, false);
chBSemObjectInit(&_io_semaphore, false);
#ifndef HAL_NO_MONITOR_THREAD
// setup the monitor thread - this is used to detect software lockups
_monitor_thread_ctx = chThdCreateStatic(_monitor_thread_wa,
sizeof(_monitor_thread_wa),
APM_MONITOR_PRIORITY, /* Initial priority. */
_monitor_thread, /* Thread function. */
this); /* Thread parameter. */
#endif
#ifndef HAL_NO_TIMER_THREAD
// setup the timer thread - this will call tasks at 1kHz
_timer_thread_ctx = chThdCreateStatic(_timer_thread_wa,
sizeof(_timer_thread_wa),
APM_TIMER_PRIORITY, /* Initial priority. */
_timer_thread, /* Thread function. */
this); /* Thread parameter. */
#endif
#ifndef HAL_NO_RCOUT_THREAD
// setup the RCOUT thread - this will call tasks at 1kHz
_rcout_thread_ctx = chThdCreateStatic(_rcout_thread_wa,
sizeof(_rcout_thread_wa),
APM_RCOUT_PRIORITY, /* Initial priority. */
_rcout_thread, /* Thread function. */
this); /* Thread parameter. */
#endif
#ifndef HAL_NO_RCIN_THREAD
// setup the RCIN thread - this will call tasks at 1kHz
_rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
sizeof(_rcin_thread_wa),
APM_RCIN_PRIORITY, /* Initial priority. */
_rcin_thread, /* Thread function. */
this); /* Thread parameter. */
#endif
#ifndef HAL_USE_EMPTY_IO
// the IO thread runs at lower priority
_io_thread_ctx = chThdCreateStatic(_io_thread_wa,
sizeof(_io_thread_wa),
APM_IO_PRIORITY, /* Initial priority. */
_io_thread, /* Thread function. */
this); /* Thread parameter. */
#endif
#ifndef HAL_USE_EMPTY_STORAGE
// the storage thread runs at just above IO priority
_storage_thread_ctx = chThdCreateStatic(_storage_thread_wa,
sizeof(_storage_thread_wa),
APM_STORAGE_PRIORITY, /* Initial priority. */
_storage_thread, /* Thread function. */
this); /* Thread parameter. */
#endif
}
2.2.2 AP_HAL_Linux
使用pthread_create建立以下线程：
- main_loop // 主应用线程
- timer
- uart
- rcin
- io
void Scheduler::init()
{
int ret;
const struct sched_table {
const char *name;
SchedulerThread *thread;
int policy;
int prio;
uint32_t rate;
} sched_table[] = {
SCHED_THREAD(timer, TIMER),
SCHED_THREAD(uart, UART),
SCHED_THREAD(rcin, RCIN),
SCHED_THREAD(io, IO),
};
_main_ctx = pthread_self();
init_realtime();
init_cpu_affinity();
/* set barrier to N + 1 threads: worker threads + main */
unsigned n_threads = ARRAY_SIZE(sched_table) + 1;
ret = pthread_barrier_init(&_initialized_barrier, nullptr, n_threads);
if (ret) {
AP_HAL::panic("Scheduler: Failed to initialise barrier object: %s",
strerror(ret));
}
for (size_t i = 0; i < ARRAY_SIZE(sched_table); i++) {
const struct sched_table *t = &sched_table[i];
t->thread->set_rate(t->rate);
t->thread->set_stack_size(1024 * 1024);
t->thread->start(t->name, t->policy, t->prio);
}
#if defined(DEBUG_STACK) && DEBUG_STACK
register_timer_process(FUNCTOR_BIND_MEMBER(&Scheduler::_debug_stack, void));
#endif
}
2.2.3 AP_HAL_ESP32
采用xTaskCreate建立以下ESP任务：
- _main_thread
- _timer_thread
- _rcout_thread
- _rcin_thread
- _uart_thread
- _io_thread
- _storage_thread
void Scheduler::init()
{
#ifdef SCHEDDEBUG
printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__);
#endif
//xTaskCreatePinnedToCore(_main_thread, "APM_MAIN", Scheduler::MAIN_SS, this, Scheduler::MAIN_PRIO, &_main_task_handle, 0);
xTaskCreate(_main_thread, "APM_MAIN", Scheduler::MAIN_SS, this, Scheduler::MAIN_PRIO, &_main_task_handle);
xTaskCreate(_timer_thread, "APM_TIMER", TIMER_SS, this, TIMER_PRIO, &_timer_task_handle);
xTaskCreate(_rcout_thread, "APM_RCOUT", RCOUT_SS, this, RCOUT_PRIO, &_rcout_task_handle);
xTaskCreate(_rcin_thread, "APM_RCIN", RCIN_SS, this, RCIN_PRIO, &_rcin_task_handle);
xTaskCreate(_uart_thread, "APM_UART", UART_SS, this, UART_PRIO, &_uart_task_handle);
xTaskCreate(_io_thread, "APM_IO", IO_SS, this, IO_PRIO, &_io_task_handle);
xTaskCreate(_storage_thread, "APM_STORAGE", STORAGE_SS, this, STORAGE_PRIO, &_storage_task_handle); //no actual flash writes without this, storage kinda appears to work, but does an erase on every boot and params don't persist over reset etc.
// xTaskCreate(_print_profile, "APM_PROFILE", IO_SS, this, IO_PRIO, nullptr);
//disableCore0WDT();
//disableCore1WDT();
}
2.3 driver specific threads
可以创建特定于驱动程序的线程，以支持异步处理。目前，只能以依赖于平台的方式创建特定于驱动程序的线程。
因此只有当驱动程序打算只在一种类型板上运行时，这才是合适的。
如果希望它在多个AP_HAL目标上运行，那么有两个选择：
- 可以使用register_o_process()和register_timer_process()调度程序调用来使用现有的定时器或io线程
- 可以添加一个新的HAL接口，该接口提供了在多个AP_HAL目标上创建线程的通用方法
2.4 ardupilot drivers versus platform drivers
这里主要是一个历史问题，从飞控的角度只要获取到数据，通过front-end接口被飞控应用调用到即可。
而底层是采用Andrnio方式开发的back-end驱动程序，还是通过平台原生态驱动程序获取到数据。这并不是应用关心的重点，当然这里也存在代码一定程度上的复杂。
因为ArduPilot文档中也指出：a) ArduPilot需要与PX4的团队紧密合作；b) 有些代码经过测试，稳定可靠的。如果我们都要按照设计框架去重构，需要的是人力和时间。
Well, It’s probably to say “it works!”.
ArduPilot drivers
采用hal方式开发的驱动程序。
platform drivers
平台原生态驱动程序，比如：linux驱动程序。
2.5 platform specific threads and tasks
从资料上看，之前ArduPilot代码应该使用了PX4的部分代码，可能还使用了Nuttx系统，因此除了代码中启动的线程以外，还有小系统的应用程序也会随着启动脚本启动。
- PX4模块设计之十：PX4启动过程
- PX4模块设计之十一：Built-In框架
就目前最新的ArduPilot代码看，AP_HAL_ChibiOS/AP_HAL_ESP32应该没有。当然Linux最小系统一定是有启动脚本的，而Linux系统中，飞控仅仅是其中的一个进程（多个线程）应用。
2.6 the AP_Scheduler system
主线程下会调度AP_Scheduler
AP_Vehicle::setup
├──> get_scheduler_tasks(tasks, task_count, log_bit);
└──> AP::scheduler().init(tasks, task_count, log_bit);
以下就是ArduPilot的主要应用：
/*
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
};
注：关于AP_Scheduler后续我们专题研讨，从上面的列表，可以看出基本上飞控的功能都在这里呈现了。
AP_Scheduler的一些主要注意事项：
- 不应该阻塞(除了ins.update()调用)
- 不应该调用sleep函数(就像飞行员飞行时不能睡觉一样)
- 应该有可预测的最坏情况
2.7 Resource Exclusion & Data Consistency
资源互斥的主要问题是竞争导致的，比如：I2C总线获取传感数据（始终只有一个从设备能响应，不能并行。）
而数据一致性的问题，读写数据需要确保原子操作，比如：short(2 Bytes)，不能分开两次获取。要一次性读完或者写完。
- semaphores: 采用互斥保护资源，时间换空间
- lockless data structures: 采用类似ring_buff的概念，牺牲空间换时间
3. Threading概念
回到我们关于ArduPilot的Threading概念上，工程项目涉及各种平台AP_HAL_ChibiOS/AP_HAL_Linux/AP_HAL_ESP32/AP_HAL_Empty/AP_HAL_SITL，为此我们不能单一的理解这里的Threading概念。
从实际代码的角度，可以认为类似嵌入式实时系统任务的概念。
- bear metal: 过程/函数
- RTOS: 任务
- Linux: 线程
- ESP32: 任务
注:其中各个任务的内存空间是共享的，因此可以使用semaphores和lockless data structures。如果任务内存空间不共享，就需要使用到其他的IPC方式了，比如uORB/ivy。
4. 参考资料
【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】How Threads Run in Ardupilot in Linux (AP_HAL_Linux)?