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 <pthread>(20005400)
 39 60 FIFO PTHREAD READY <pthread>(20005400)
 40 59 FIFO PTHREAD WAITSEM <pthread>(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源码的东西不会忘记的太多了，哈哈哈。

---