Ardupilot -- APM源码笔记四（重制）~ 线程机制

认识Ardupilot线程

在了解过Ardupilot的链接库之后，是时候来认识一下Ardupilot是怎么处理线程了，对于从arduino继承过来的setup（）/loop（）架构，会让我们认为Ardupilot也是一个单线程系统，事实上并非如此
Ardupilot线程处理方式还是取决于控制板，例如APM1板、APM2板不支持多线程，需要配置定时器来实现定时回调，PX4板、Linux板支持POSIX多线程的实时调度，在Ardupilot源码中被经常使用

---

学习Ardupilot线程需要先了解几个概念~

• 定时回调
• 硬件平台专有线程
• 驱动专有线程
• Ardupilot驱动与系统驱动
• 系统专有线程与任务
• AP_Scheduler系统
• 信号量
• lockless data structures（无锁的数据结构？！）

1、定时回调

在AP_HAL中，每个飞控平台都提供了一个1khz的定时器，任何程序需要用到1khz的速率调用都可以通过创建定时器来实现，所有被创建的定时器都有序排列，简单而有效的原始机制，创建的定时回调函数如下
//7

hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_update));

摘取自MS5611气压计的驱动例子，宏AP_HAL_MEMBERPROC()提供了一个方法来封装C++成员函数的作为回调参数（用函数指针做链接），如果有函数调用小于1Khz的，应该做好last_called参数的记录，不够回调时间立刻返回，也可以调用hal.scheduler->millis( )和hal.scheduler->micros( )函数，利用微妙跟毫秒来控制时间调度，可以在已有的任务例程做修改或者新建一个定时回调的例程，创建一个增量计数定时器，实现一个每秒打印的功能，或修改你需要的时间计数机制来实现其他功能

2、硬件平台专有线程

每个平台都有其专属的线程，AP_HAL平台创建若干（平台）线程来对应它们的基本操作，例如PX4的专有线程~

• 串口线程，实现串口及USB口读写
• 计时器线程，上面提到的提供1khz的定时器
• IO线程，支持microSD 卡, EEPROM 和 FRAM写入

去看一下AP_HAL里每个Scheduler.cpp是怎么创建线程的跟每个线程的优先级，如果你手头有一块px4板，用数据线连接到控制台终端调试口（串口5，后续章节有做串口讲解），比特率57600，连上后，尝试在终端键入 ps 指令后能收到以下信息~

PID PRI SCHD TYPE NP STATE NAME
 0 0 FIFO TASK READY Idle Task()
 1 192 FIFO KTHREAD WAITSIG hpwork()
 2 50 FIFO KTHREAD WAITSIG lpwork()
 3 100 FIFO TASK RUNNING init()
 37 180 FIFO TASK WAITSEM AHRS_Test()
 38 181 FIFO PTHREAD WAITSEM (20005400)
 39 60 FIFO PTHREAD READY (20005400)
 40 59 FIFO PTHREAD WAITSEM (20005400)
 10 240 FIFO TASK WAITSEM px4io()
 13 100 FIFO TASK WAITSEM fmuservo()
 30 240 FIFO TASK WAITSEM uavcan()

上述的AHRS_Test( )线程运行在 libraries/AP_AHRS/examples/AHRS_Test示例中，还能得知各线程优先级，计时器线程（优先级181），UART线程（优先级60）和IO线程（优先级59），另外还有 px4io， fmuservo，uavcan， lpwork， hpwork跟一些闲置任务，其他平台也根据自身需要创建了相对应的线程
一些共用线程会执行低频率的任务，不影响到Ardupilot主进程运行，举个栗子，AP_Terrain 库需要在microSD 卡生成文件（存储及检索地形文件），所用方式是调用hal.scheduler->register_io_process( )函数，像这样~

hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Terrain::io_timer));

AP_Terrain::io_timer函数定期执行，用于平台IO线程（优先级59）中，意味着这是IO任务中一个较低优先级的实时调用，比较重要的一点，IO任务并不是又定时器线程所调用的，因为这会导致（获取）高速运行的传感器数据出现延时的状况

3、驱动程序专有线程

驱动程序线程同样很重要，针对每个设备驱动的异步处理，当前只能根据所依赖的平台创建驱动程序，这对于你的驱动只运行在其中一种平台来说是个好办法，如果想要驱动程序可以运行在多个AP_HAL平台的，有两种办法~

• 利用register_io_process( ) 和 register_timer_process( )来调用当前定时器或IO线程
• 创建一个新的HAL接口，提供一个通用方式生成多个AP_HAL目标线程

驱动程序的一个例子是Linux平台的ToneAlarm线程，可参考AP_HAL_Linux/ToneAlarmDriver.cpp

4、 Ardupilot（通用）驱动与平台驱动

在源码中，一个驱动在多个地方有配置文件，例如MPU6000驱动配置，在libraries/AP_InertalSensor/AP_InertialSensor_MPU6000.cpp，还有另一个在PX4Firmware/src/drivers/mpu6000，出现重复配置的原因是，源码中提供了一组测试硬件的驱动程序（可参考硬件抽象层原理），检测控制板做相应配置
对照着Ardupilot库接口的PX4驱动简介，当我们在PX4平台配置时PX4驱动会生产一个“shim”驱动程序，存在于ibraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp，它会检测PX4平台所需的IMU系统和尽可能把它所得到的作为AP_InertialSensor库的一部分
所以如果我们硬件搭载了一个MPU6000，在非PX4平台上将配置AP_InertialSensor_MPU6000.cpp文件，在PX4平台配置AP_InertialSensor_PX4.cpp文件
向东类型的驱动程序可以服务在不同的AP_HAL端口上，在Linux板上我们用Linux内核驱动程序来服务一些传感器，其他传感器我们可以从通用的SPI、I2C接口调用文件树中相应的驱动程序用于不同的控制板

5、平台专用驱动及任务

在一些平台启动过程中有部分的任务及线程被创建，这些都是非常具有特定性的，以下将讲述基于PX4板的任务，在上面“ps”输出列表中，由AP_HAL_PX4调度程序来开始这些任务及线程，具体点说~

• idle task - 空闲任务，没有其他运行的时候被调用
• init - 用于启动系统及一些初始化
• px4io - 处理与PX4IO协同处理器的通讯
• hpwork - 处理基于PX4的驱动线程（主要是I2C驱动的）
• lpwork - 处理一些低优先级任务（例如 IO）
• fmuservo - 处理FMU的PWM输出对接
• uavcan - 处理uavcan CANBUS协议

所有的启动任务都由PX4的特定脚本（rc.APM）来控制，这个脚本在控制板上电时运行，作为练习，可以尝试编辑rc.APM脚本，加入一些sleep和echo命令，重新烧录固件后可以在debug串口获得测试信息，另一个探索PX4启动流程的法子是拔掉microSD卡，当检测到有microSD卡是会紧跟rc.APM脚本后运行rcS脚本，假如没检测到SD卡将从USB调试口输出一个空的nsh类型信息到控制台，这时可以手动单步调试rc.APM的每个步骤到控制台，借此来了解整个启动流程

在PIXHAWK无microSD卡启动时尝试以下练习，连接USB查看控制台输出~

tone_alarm stop
uorb start
mpu6000 start
mpu6000 info
mpu6000 test
mount -t binfs /dev/null /bin
ls /bin
perf

6、AP_Scheduler系统

Scheduler库用于在主线程中的时间控制，同时通过在AP_Scheduler中给不同线程任务划分执行时间，简单的说就是一套多线程轮循机制，控制每一个线程的时间周期及运行频率。在每个固件源码的Loop（）函数中都包含这些代码：

• 等待一个新的IMU采样
• 在IMU的采样期间执行其他任务

代码变现形式就类似一个驱动表格，这在每套飞行源码中的AP_Scheduler::Task table都有体现，你也可以先从示例代码中 AP_Scheduler/examples/Scheduler_test.cpp做个简单的了解，一个小表格，3组调度任务，每个任务3个参数~

static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 { ins_update, 1, 1000 },
 { one_hz_print, 50, 1000 },
 { five_second_call, 250, 1800 },
};

每个任务的第一个参数表示该任务的调用函数，第一个数字代表调用频率，在ins.init()函数中设置了50HZ为一个调度单位，即20ms。这表示ins_update函数的调用为20ms一次，one_hz_print函数的调用50 * 20ms = 1s一次，当然five_second_call函数就是250 * 20ms = 5s调用一次了。第二个数字为每个任务的最大执行时间，scheduler.run()函数为每个任务预留了充足的执行时间，在该时间内任务执行完将直接跳到下一个任务调度，如果超过该时间仍无法完成任务函数的，将pass掉这个任务，直接下一任务的执行。另一个要点是ins.wait_for_sample()函数，它就像一个节拍器驱动着ArduPilot的任务调度，它在新的IMU有效取样之前阻碍主线程的调用，直到拿到有效的IMU取样，这个简单理解下就行了，而IMU取样的间隔时间由ins.init()控制调用。

注意在AP_Scheduler中的任务需具备以下特征~

• 除了ins.update()的调用，它们不能造成程序阻塞
• 不能在飞行（模式）时调用sleep相关函数 （autopilot就像一个真正的驾驶员，不能在飞行的时候睡着了 =。=）
• 它们需要有执行的最长预估时间

你可以在Scheduler_test的实例中尝试去修改任务内容跟添加自己的任务，先做点简单的~

• 读取气压计数值
• 读取指南针数值
• 读取GPS数值
• 更新AHRS（姿态导航系统）和输出roll/pitch（横滚角、俯仰角）

看看每个库sketches示例，在教程的前面篇章有提到如何去查找传感器库。
不过这些对于刚接触的你应该有点困难，因为要先找到对应传感器的驱动，找他们的数据输出函数及调用方式，比较适合新手的是先找找蜂鸣器的控制，在events的类里面，能看到有不同事件对应的蜂鸣器叫声，开锁、解锁、更换飞行模式，AP_Notify::events.user_mode_change = 1;这里给出一条更换飞行模式的蜂鸣器叫声代码，是我在调试代码的时候经常用到的手段，在新写代码中插入一个声音提示，在一些不必要起飞升空才能跑到的代码中，你可以知道你写的代码有没有被执行过。

7、信号量

当有多个线程（或定时器回调）需要共享同样的数据结构或更新数据，就要注意到冲突问题了，在ArduPilot中有三个方法来解决这样的冲突：信号量、lockless data（无锁的数据？）和PX4 ORB。AP_HAL信号量运行在可用信号系统的任何特定平台上，提供一个简单的互斥机制，例如，I2C设备可以请求一个I2C总线信号量，以确保一次只有一个I2C设备在使用，可以参考 libraries/AP_Compass/AP_Compass_HMC5843.cpp的HMC5843驱动代码，跟get_semaphore()的函数实现，理解信号量的好处。

8、Lockless Data Structures

ArduPilot源码也包含了lockless data示例来避免访问冲突的场合，这要比信号量机制更有效率，可参考源码中的两个示例~

• libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp的_shared_data结构
• 多数地方用到的环形缓冲区，例如libraries/DataFlash/DataFlash_File.cpp

这两个示例都证明了lockless data是比较安全的并行访问机制，对于DataFlash_File注意_writebuf_head和_writebuf_tail变量的使用。

9、PX4 ORB(Object Request Broker)

仿信号机制的另一种方式是PX4 ORB，PX4的一种互斥机制。
另外两种PX4驱动通信机制，如下~

• ioctl 调用 (详见 AP_HAL_PX4/RCOutput.cpp示例)
• /dev/xxx read/write 调用 (见 _timer_tick in AP_HAL_PX4/RCOutput.cpp)

---

NOTE

近期有些忙，转入了新项目，所以本篇教程也是写了一半中途停了一段时间，可能最近不会有新的篇章加入了，后续有时间我会尽快把剩下的都给补上，希望到那会儿Ardupilot源码的东西不会忘记的太多了，哈哈哈。

---