ArduPilot 学习之路 - 6,线程
英文原文地址:https://ardupilot.org/dev/docs/learning-ardupilot-threading.html
理解 ArduPilot 线程
线程时 ArduPilot 运行的核心概念,setup() / loop() 结构来源于 Arduino,使得 ArduPilot 系统看起来更像是单线程系统,但事实并非如此。
ArduPilot 中的线程处理方法取决于其所运行的主板。有些主板(例如APM1和APM2)不支持线程,因此可以使用简单的计时器和回调。某些主板(PX4和Linux)支持具有实时优先级的丰富Posix线程模型,ArduPilot广泛使用了这些模型。
ArduPilot 线程主要涉及到如下关键概念:
- The timer callbacks 定时器回调
- HAL specific threads HAL特定线程
- driver specific threads 驱动程序特定线程
- ardupilot drivers versus platform driversardupilot 驱动程序与平台驱动程序
- platform specific threads and tasks 平台特定的线程和任务
- the AP_Scheduler system AP_调度系统
- semaphores 信号量
- lockless data structures 无锁数据结构
1,定时器回调(The timer callbacks)
每个平台在 AP_HAL 中都提供一个1kHz计时器。 ArduPilot 中的任何代码都可以注册一个计时器功能,然后以1kHz的频率调用该功能。所有已注册的计时器功能均被依次调用。这种机制很原始,但是它具备很强的移植性,而且非常有用。您可以通过调用 hal.scheduler-> register_timer_process()来注册计时器回调,如下所示:
该示例来自MS5611气压计驱动程序。 AP_HAL_MEMBERPROC()宏提供了一种将C ++成员函数封装为回调参数的方法。
当一段代码希望某件事发生在低于1kHz的频率时,它应该保持其自己的 “ last_ Called” 变量,并在没有足够时间的情况下立即返回。您可以使用 hal.scheduler-> millis()和 hal.scheduler-> micros()函数来获取自启动以来的时间,该时间以毫秒和微秒为单位。
现在,您应该去修改一个现有的示例(或创建一个新的示例)并添加一个计时器回调。使计时器递增一个计数器,然后在loop()函数中每秒打印一次计数器的值。修改函数,使其每25毫秒递增一次计数器。
2,HAL特定线程
在支持实线程的平台上,该平台的 AP_HAL 将创建多个线程以支持基本操作。例如在Pixhawk上创建以下特定于HAL的线程:
- UART线程,用于读写UART(和USB)
- 计时器线程,支持上述 1kHz 计时器功能
- IO线程,支持写入 microSD 卡,EEPROM 和 FRAM
在每个AP_HAL实现中查看 Scheduler.cpp,以查看创建了哪些线程以及每个线程的实时优先级。
如果你的硬件为 Pixhawk,那么设置调试控制台电缆并连接到nsh控制台(serial5 端口)。连接速度为57600。连接后,尝试“ ps ”命令广告,将获得以下内容:
在此示例中,您可以看到“ AHRS_Test”线程,该线程正在运行来自库/ AP_AHRS / examples / AHRS_Test的示例,还可以看到计时器线程(优先级181),UART线程(优先级60)和IO线程(优先级59)。其他AP_HAL端口具有更多或更少的线程,具体取决于所需的线程。
线程为控制器提供了一种常用的方法,可以在不中断主要自动驾驶飞行代码的情况下安排慢任务的方式。例如,AP_Terrain库需要能够对microSD卡执行文件 IO(以存储和检索地形数据)。它的执行方式是这样调用函数 hal.scheduler-> register_io_process():
设置AP_Terrain :: io_timer函数以使其定期调用。在板卡 IO 线程中调用意味着它的实时优先级较低,适合存储IO任务。请勿在计时器线程上调用此类缓慢的IO任务,因为它们会导致更重要的高速传感器数据处理过程中的延迟,这一点很重要
3,驱动程序特定线程(Driver specific threads)
对于特定的异步处理,可以创建特定的驱动线程。目前,只能以依赖于平台的方式创建特定于驱动程序的线程,因此,仅驱动程序仅打算在一种类型的自动驾驶板上运行时才适用。如果希望它在多个AP_HAL目标上运行,则有两种选择:
- 可以使用 register_io_process()和 register_timer_process()调度程序调用来使用现有的计时器或IO线程
- 可以添加一个新的HAL接口,该接口提供了一种在多个 AP_HAL 目标上创建线程的通用方法
驱动程序特定线程的一个示例是 Linux 端口中的 ToneAlarm 线程。参见 AP_HAL_Linux / ToneAlarmDriver.cpp
4,驱动程序与平台驱动程序(ArduPilot drivers versus platform drivers)
细心的程序员可能会会注意到 ArduPilot 中有一些驱动程序重复。例如,库 /AP_InertalSensor/AP_InertialSensor_MPU6000.cpp 中有一个MPU6000驱动程序,PX4Firmware / src / drivers / mpu6000 中有另一个 MPU6000 驱动程序 。
重复的原因是PX4项目已经为Pixhawk板卡随附的硬件提供了一组经过良好测试的驱动程序,并且我们与PX4团队在开发和增强这些驱动程序方面享有良好的合作关系。因此,当我们为PX4构建ArduPilot时,我们通过编写小的 “shim” 驱动程序来利用PX4驱动程序,这些驱动程序为PX4驱动程序提供了标准 ArduPilot 库接口。如果查看 libraries / AP_InertialSensor / AP_InertialSensor_PX4.cpp,您会看到一个小的填充驱动程序,询问PX4该板上有哪些IMU驱动程序,并自动将它们作为ArduPilot AP_InertialSensor 库的一部分提供。
因此,如果板上有 MPU6000,则在非 Pixhawk / NuttX 平台上使用AP_InertialSensor_MPU6000.cpp 驱动程序,在基于NuttX的平台上使用AP_InertialSensor_PX4.cpp 驱动程序。
其他 AP_HAL 端口也可能发生相同类型的拆分。例如,我们可以将Linux内核驱动程序用于Linux板上的某些传感器。对于其他传感器,我们使用通用的 AP_HAL I2C 和 SPI 接口来使用 ArduPilot“ 树内”驱动程序,该驱动程序可在各种板上使用。
5,平台特定的线程和任务(Platform specific threads and tasks)
在某些平台上,启动过程将创建许多基本任务和线程。这些是特定于平台的,因此为了本教程的缘故,我将专注于基于PX4的板上使用的任务。
在上面的“ ps”输出中,我们看到了许多 AP_HAL_PX4 Scheduler 代码未启动的任务和线程。具体来说,它们是:
- idle task - called when there is nothing else to run
- init - used to start up the system
- px4io - handle the communication with the PX4IO co-processor
- hpwork - handle thread based PX4 drivers (mainly I2C drivers)
- lpwork - handle thread based low priority work (eg. IO)
- fmuservo - handle talking to the auxillary PWM outputs on the FMU
- uavcan - handle the uavcan CANBUS protocol
所有这些任务的启动由特定于 PX4 的 rc.APM 脚本控制。该脚本在PX4引导时运行,并负责检测我们正在使用哪种PX4板,然后为该板加载正确的任务和驱动程序。它是一个“ nsh” 脚本,类似于bourne shell脚本(尽管nsh更原始)。
作为练习,请尝试编辑 rc.APM 脚本并添加一些sleep和echo命令。然后,在主板启动时上传新固件并连接到调试控制台。您的 echo 命令应显示在控制台上。
探索PX4启动的另一种非常有用的方法是在插槽中没有 microSD 卡的情况下启动。 rcS 脚本运行在 rc.APM 之前,它检测是否插入了 microSD,并在 USB 端口上提供一个原始的 nsh 控制台。然后,还可以在 USB 控制台上自己手动运行 rc.APM 的所有步骤,以了解其工作方式。
在没有 microSD 卡的情况下启动 Pixhawk 并连接到 USB 控制台后,请尝试以下练习:
tone_alarm stop
uorb start
mpu6000 start
mpu6000 info
mpu6000 test
mount -t binfs /dev/null /bin
ls /bin
perf
尝试运行其他驱动,在 / bin 中查看可用的内容。这些命令大多数的源代码在 PX4Firmware / src / drivers 中。浏览一下mpu6000 驱动程序,以了解所涉及的内容。
鉴于我们的主题是线程和任务,因此值得一提的是 PX4Firmware git 树中的线程的简短描述。如果您查看 mpu6000 驱动程序,您将看到如下一行:
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
这等效于 AP_HAL 中的 hal.scheduler-> register_timer_process()函数,但特定于PX4,并且更加灵活。它表示希望PX4的HRT(高分辨率计时器)子系统每1000微秒调用一次MPU6000 :: measure_trampoline 函数。使用 hrt_call_every()是用于操作非常快速的驱动程序(例如SPI设备驱动程序)的常用方法。这些操作通常在禁用中断的情况下运行,并且最多只需要几十微秒。
如果将此与hmc5883驱动程序进行比较,将看到如下一行:
work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
对于速度较慢的设备(例如I2C设备),常规做法时使用替代机制。这样做是将 cycle_trampoline 函数添加到您在上面看到的HPWORK 线程内的工作队列中。在 HPWORK Worker 中进行的调用应在启用中断的情况下运行,并且可能需要花费数百微秒的时间。对于将花费比应使用LPWORK工作队列更长的时间的任务,该任务将在较低优先级的 lpwork 线程中运行。
6,AP_调度系统(The AP_Scheduler system)
对于 ArduPilot 线程任务,需要了解的下一个方面是 AP_Scheduler 系统。 AP_Scheduler 库用于在主无人设备线程中分配时间,同时提供一些简单的机制来控制每个操作使用多少时间(在 AP_Scheduler 中称为“任务”)。
它的工作方式是每个设备的 loop()函数都包含一些执行此操作的代码:
- 等待新的 IMU 采样数据到达
- 在每个 IMU 样本之间调用一组任务
每种无人机设备都有一个 AP_Scheduler :: Task 表。要了解其工作原理,请查看 AP_Scheduler / examples / Scheduler_test.cpp 示例。如果查看该文件,您将看到一个小表格,其中包含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()调用控制的单位。对于此示例,ins.init()使用 RATE_50HZ,因此每个调度步骤为20ms。这意味着每 20 毫秒进行一次ins_update()调用,每50次(即每秒一次)调用一次 one_hz_print()函数,每250次(即每5秒一次)调用一次 five_second_call()。
第三个数字是该功能预计要花费的最长时间。除非在此调度运行中剩余足够的时间来运行该函数,否则这样做可以避免进行调用。当调用 scheduler.run()时,将传递可用于运行任务的时间(以微秒为单位),如果最差的情况下该任务的时间意味着该任务在该时间用完之前不适合,则不会调用该程序。
需要注意的另一点是 ins.wait_for_sample()调用。那就是在 ArduPilot 中推动调度的“节拍器”。在新的 IMU 样本可用之前,它将阻止执行无人机主线程, IMU样本之间的时间由 ins.init()调用的参数控制。
注意 AP_Scheduler 表中的任务必须具有以下属性:
- 它们永远都不会阻塞(除了ins.update()调用之外)
- 他们绝不应该在飞行时调用睡眠功能(像真正的飞行员一样,自动驾驶仪在飞行时也不应睡眠)
- 他们应该有可预测的最坏情况时机
现在,我们可以去去修改 Scheduler_test 示例,并添加自己的任务以运行,尝试添加执行以下操作的任务:
- 读取气压计
- 读取指南针
- 读取 GPS 信息
- 更新 AHRS 并打印滚动 / 俯仰角度
查看本教程前面使用的每个库的示例,以了解如何使用每个传感器库。
7,信号量(Semaphores)
当有多个线程(或计时器回调)时,需要确保两个执行逻辑线程共享的数据结构以防止损坏的方式进行更新。在 ArduPilot中,有 3 种基本方法可以做到这一点:a,信号量;b,无锁数据结构;c,PX4 ORB。
AP_HAL 信号量只是特定平台上可用的任何信号量系统的包装,提供了一种相互排斥的简单机制。例如,I2C 驱动程序使用 I2C 总线信号量,以确保一次仅使用一个I2C设备,防止数据冲突。
读者可以转到库 /AP_Compass/AP_Compass_HMC5843.cpp 中的 hmc5843 驱动程序,并查找 get_semaphore()调用。查看所有使用它的地方,看看是否可以弄清楚为什么需要它。
8,无锁数据结构(Lockless Data Structures)
ArduPilot 代码还包含使用无锁数据结构来避免使用信号量的示例,这比信号量更加有效。
提供两个示例参考:
- the _shared_data structure in libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
- the ring buffers used in numerous places. A good example is libraries/DataFlash/DataFlash_File.cpp
参考这两个例子,理解示例并发访问如何保证数据安全。对于 DataFlash_File,请查看_writebuf_head 和 _writebuf_tail 变量的使用。最好在ArduPilot中的多个位置创建一个通用的环形缓冲区类,以代替单独的环形缓冲区实现。
9,The PX4 ORB
这种机制的另一个示例是PX4 ORB。 ORB(对象请求代理)是一种使用在多线程环境中安全的发布/订阅模型,将数据从系统的一个部分提供给另一部分(例如,设备驱动程序->车辆代码)的方法。
ORB 提供了一种很好的机制来声明这种共享的结构(全部在PX4Firmware / src / modules / uORB /中定义)。然后,代码可以将数据“发布”到这些主题之一,而其他代码则可以选择这些主题。
一个示例是舵机或者电调控制量的发布,以便可以在Pixhawk上使用 uavcan ESC。参考 AP_HAL_PX4 / RCOutput.cpp 中的_publish_actuators()函数。您会看到它发布了一个“ actuator_direct”主题,其中包含每个ESC所需的速度。 uavcan在PX4Firmware / src / modules / uavcan / uavcan_main.cpp中对这些监视代码进行编码,以查找对此主题的更改,并将新值输出到uavcan ESC。
PX4驱动程序进行通信的其他两种常见机制是:
- ioctl 调用(请参阅AP_HAL_PX4 / RCOutput.cpp中的示例)
- /dev/xxx read/write calls (see _timer_tick in AP_HAL_PX4/RCOutput.cpp)