ArduPilot之开源代码基础知识&Threading概念

1. 源由

Ardunio的setup()/loop()设计概念比较适合独立功能级别的验证和测试。

相对于飞控复杂系统来说，就需要多线程/多任务的加持，为此，使用支持具有实时优先级的丰富的Posix线程模型就更为重要。

因此本章将就ArduPilot的基础知识和Threading概念做一个了解。

从AP_HAL硬件解耦设计看，ArduPilot有以下几种HAL分层：

1. AP_HAL_ChibiOS
2. AP_HAL_Linux
3. AP_HAL_ESP32
4. AP_HAL_Empty
5. 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建立以下线程：

0. main_loop // 主应用线程
1. _monitor_thread_wa
2. _timer_thread_wa
3. _rcout_thread_wa
4. _rcin_thread_wa
5. _io_thread_wa
6. _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建立以下线程：

0. main_loop // 主应用线程
1. timer
2. uart
3. rcin
4. 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任务：

1. _main_thread
2. _timer_thread
3. _rcout_thread
4. _rcin_thread
5. _uart_thread
6. _io_thread
7. _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目标上运行，那么有两个选择：

1. 可以使用register_o_process()和register_timer_process()调度程序调用来使用现有的定时器或io线程
2. 可以添加一个新的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)?