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_STACKregister_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 SCHEDDEBUGprintf("%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 determinethe order in which tasks are run. Convenience methods SCHED_TASKand 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 populatedFAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),// run low level rate controllers that only require IMU dataFAST_TASK(run_rate_controller),
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLEDFAST_TASK(run_custom_controller),
#endif
#if FRAME_CONFIG == HELI_FRAMEFAST_TASK(heli_update_autorotation),
#endif //HELI_FRAME// send outputs to the motors library immediatelyFAST_TASK(motors_output),// run EKF state estimator (expensive)FAST_TASK(read_AHRS),
#if FRAME_CONFIG == HELI_FRAMEFAST_TASK(update_heli_control_dynamics),
#endif //HELI_FRAME// Inertial NavFAST_TASK(read_inertia),// check if ekf has reset target heading or positionFAST_TASK(check_ekf_reset),// run the attitude controllersFAST_TASK(update_flight_mode),// update home from EKF if necessaryFAST_TASK(update_home_from_EKF),// check if we've landed or crashedFAST_TASK(update_land_and_crash_detectors),// surface tracking updateFAST_TASK(update_rangefinder_terrain_offset),
#if HAL_MOUNT_ENABLED// camera mount's fast updateFAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
#endifFAST_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_ENABLEDSCHED_TASK_CLASS(AP_OpticalFlow, &copter.optflow, update, 200, 160, 12),
#endifSCHED_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 == ENABLEDSCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50, 24),
#endifSCHED_TASK(auto_disarm_check, 10, 50, 27),SCHED_TASK(auto_trim, 10, 75, 30),
#if RANGEFINDER_ENABLED == ENABLEDSCHED_TASK(read_rangefinder, 20, 100, 33),
#endif
#if HAL_PROXIMITY_ENABLEDSCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36),
#endif
#if BEACON_ENABLED == ENABLEDSCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50, 39),
#endifSCHED_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 == ENABLEDSCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100, 51),
#endif
#if HAL_SPRAYER_ENABLEDSCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90, 54),
#endifSCHED_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 == ENABLEDSCHED_TASK(update_precland, 400, 50, 69),
#endif
#if FRAME_CONFIG == HELI_FRAMESCHED_TASK(check_dynamic_flight, 50, 75, 72),
#endif
#if LOGGING_ENABLED == ENABLEDSCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75),
#endifSCHED_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_ENABLEDSCHED_TASK(landinggear_update, 10, 75, 93),
#endifSCHED_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_ENABLEDSCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75, 108),
#endif
#if AP_CAMERA_ENABLEDSCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111),
#endif
#if LOGGING_ENABLED == ENABLEDSCHED_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),
#endifSCHED_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_ENABLEDSCHED_TASK_CLASS(AP_RPM, &copter.rpm_sensor, update, 40, 200, 129),
#endifSCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),
#if HAL_ADSB_ENABLEDSCHED_TASK(avoidance_adsb_update, 10, 100, 138),
#endif
#if ADVANCED_FAILSAFE == ENABLEDSCHED_TASK(afs_fs_check, 10, 100, 141),
#endif
#if AP_TERRAIN_AVAILABLESCHED_TASK(terrain_update, 10, 100, 144),
#endif
#if AP_GRIPPER_ENABLEDSCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75, 147),
#endif
#if AP_WINCH_ENABLEDSCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150),
#endif
#ifdef USERHOOK_FASTLOOPSCHED_TASK(userhook_FastLoop, 100, 75, 153),
#endif
#ifdef USERHOOK_50HZLOOPSCHED_TASK(userhook_50Hz, 50, 75, 156),
#endif
#ifdef USERHOOK_MEDIUMLOOPSCHED_TASK(userhook_MediumLoop, 10, 75, 159),
#endif
#ifdef USERHOOK_SLOWLOOPSCHED_TASK(userhook_SlowLoop, 3.3, 75, 162),
#endif
#ifdef USERHOOK_SUPERSLOWLOOPSCHED_TASK(userhook_SuperSlowLoop, 1, 75, 165),
#endif
#if HAL_BUTTON_ENABLEDSCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168),
#endif
#if STATS_ENABLED == ENABLEDSCHED_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)?