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)