Ardupilot移植經驗分享（1）簡要介紹了移植Ardupilot的思路，重點講述了下載編譯源碼的要點和搭建源碼閱讀環境的方法。下載編譯源碼，一方面是為了搭建源碼閱讀環境，另一方面是當閱讀源碼遇到疑問時，可以稍作修改后進行調試驗證。搭建便捷高效的源碼閱讀環境，更是非常重要。若是選擇的源碼閱讀器不能進行函數跳轉，不能查看函數調用棧，不能快速導航到目標函數或變量，閱讀70萬行ardupilot代碼的工作將寸步難行。這就是筆者花費很大的篇幅來講解的原因，沒看過第一篇的同學先點擊上面的鏈接噢。

下面開始閱讀源碼之旅。剛才提到，ardupilot工程有700k行代碼，這個統計出版官方開發者wiki。

雖然龐大的體量令人望而生畏，但若明確了目標，把握了脈絡，閱讀也非難事。下面讓筆者按著當初的思路，分享閱讀的步驟和要點，帶領大家體驗一次奇妙的閱讀之旅。

下面的內容分為三個部分：

明確目標

把握脈絡

深入細節

明確目標

我們的最終目標是通過底層適配的方法，移植ardupilot到自己的硬件平臺。不清楚底層適配的同學，請看Ardupilot移植經驗分享（1）的“移植Ardupilot的方法有兩種”部分。而閱讀源碼的目標，也就非常簡單，即要搞清楚如何進行適配：

要適配哪些接口（或者說函數）？

實現這些接口時，可參考哪些代碼？

會不會有難以實現的接口？

明確了這幾個問題，可以幫我們縮小閱讀的范圍。我們并不需要真去閱讀那700k行代碼，而是圍繞移植，這個中心目標，有目的有范圍地進行閱讀。

前篇文章介紹ardupilot時，提到代碼分為5個部分，筆者帶領大家回憶下最重要的3個：

載具代碼：業務層的代碼，比如飛行器初始化，飛行模式控制。

共享庫：libraries中除以AP_HAL開頭的所有子目錄。包含傳感器驅動，姿態位置估算與控制。

硬件抽象層（HAL）：libraries中以AP_HAL開頭子目錄，如AP_HAL， AP_HAL_PX4， AP_HAL_Linux。

明白了這三塊之間的相互關系，上述問題就迎刃而解。一圖以蔽之：

UML圖示說明

本文多處使用UML圖來展示ardupilot的框架和流程，有些遵循標準語法，有些則是筆者自己習慣。無論如何，為方便大家理解，筆者會在第一次出現該種圖示時做必要說明。

上述是UML組件圖，非標準語法。

實線箭頭表示使用，比如Vehicle specific flight code使用Shared Libraries。

實線表示關聯。AP_HAL_PX4實現了AP_HAL接口，準確說是繼承的關系。不過筆者在UML類圖中才見過繼承的語法，所以這里僅使用實線。

我們要實現的是AP_HAL接口，是上圖中的紅色圓圈。現有的實現有AP_HAL_PX4（對應Pixhawk平臺），AP_HAL_Linux（對應NAVIO）。我們要添加一個平臺，為圖中藍色部分，即AP_HAL_TI。

閱讀的重點也就確定了：

搞清楚AP_HAL的每一個接口，位于libraries/AP_HAL。

參考AP_HAL_PX4的實現，位于libraires/AP_HAL_PX4。

不過若想完成移植大業，僅閱讀這兩塊內容是不夠的。

AP_HAL接口被飛控業務代碼以及共享庫調用，有些接口確切的功能必須結合具體的使用場景才能搞清楚。并且，想讓飛行器飛起來，還是要跑業務代碼。在這過程中遇到異常時，若完全不了解業務代碼，那將很難定位問題。

AP_HAL_PX4依賴于底層平臺（PX4Firmware）所提供的功能。當你想參考AP_HAL_PX4是如何實現某個接口的時候，你可能會發現它只是簡單地把任務交給了PX4Firmware。當然，這只是少數情況。參考大部分的實現時，看AP_HAL_PX4本身的代碼就足夠了。

因此，除了重點閱讀AP_HAL和AP_HAL_PX4外，我們需要適當的向上和向下延伸。尤其是向上，掌握飛控業務代碼的整體框架，在需要深入某個細節時，至少要知道去哪兒看。

把握脈絡

在深入了解HAL的每一個接口之前，我們需要先了解程序的整體框架。

main函數在哪兒？

了解整體框架的第一步，莫過于找到程序入口。

主函數的定義位于ArduCopter/ArduCopter.cpp的結尾處。

AP_HAL_MAIN_CALLBACKS（&copter）;

AP_HAL_MAIN_CALLBACKS用于定義主函數。

#define AP_HAL_MAIN_CALLBACKS（CALLBACKS） extern “C” {

int AP_MAIN（int argc， char* const argv［］）;

int AP_MAIN（int argc， char* const argv［］） {

hal.run（argc， argv， CALLBACKS）;

return 0;

}

}

主函數的名稱不一定是大家熟悉的main，對于pixhawk平臺來說，其是Ardupilot_main。

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

#define AP_MAIN __EXPORT ArduPilot_main

#endif

經過預編譯后，其最終的形態如下，這就是ardupilot的主函數。

extern “C”

{

int __attribute__ （（visibility （“default”））） ArduPilot_main（int argc， char* const argv［］）;

int __attribute__ （（visibility （“default”））） ArduPilot_main（int argc， char* const argv［］）

{

hal.run（argc， argv， &copter）;

return 0;

}

}

也許你會疑惑，為什么要如此大費周章地定義主函數呢？

首先，主函數的名稱依平臺而定，這肯定是要用宏的。

其次，ardupilot項目不僅僅是ArduCopter，它還有APMrover2，AntennaTracker，ArduPlane和ArduSub，即ardupilot工程有多個入口函數，在編譯時根據目標類型來選擇。使用AP_HAL_MAIN_CALLBACKS來定義，可避免重復代碼，并且后續修改時只需要修改AP_HAL_MAIN_CALLBACKS宏即可。

放一起看下效果：

ArduCopter/ArduCopter.cpp

AP_HAL_MAIN_CALLBACKS（&copter）;

ArduSub/ArduSub.cpp

AP_HAL_MAIN_CALLBACKS（&sub）;

如果你想知道筆者是如何找到這個入口的，請看尋找ardupilot的main函數。

HAL引用

ArduPilot_main中只有一行關鍵代碼：

hal.run（argc， argv， &copter）;

hal是HAL實例的引用，這個引用定義在Copter.cpp中。

const AP_HAL：：HAL& hal = AP_HAL：：get_HAL（）;

get_HAL（）函數為業務層提供獲取HAL實例的接口，其在AP_HAL_Namespace.h中聲明，由HAL_PX4實現。

libraries/AP_HAL/AP_HAL_Namespace.h

// Must be implemented by the concrete HALs.

const HAL& get_HAL（）;

libraires/AP_HAL_PX4/HAL_PX4_Class.cpp中實現了get_HAL函數，其創建并返回HAL實例。

const AP_HAL：：HAL& AP_HAL：：get_HAL（） {

static const HAL_PX4 hal_px4;

return hal_px4;

}

HAL類是硬件抽象層（HAL）的一個集合類，它包含了對硬件抽象層其他類的實例的引用。HAL類、get_HAL（）以及HAL類引用的其他類，聲明在AP_HAL命名空間中。具體可查看AP_HAL_Namespace.h，這里就不貼代碼了，給張類圖：

HAL類定義在HAL.h中，它用于管理其他各種類的實例，從而為業務層提供便捷的訪問。

UML圖示說明

上圖為UML類圖。每一個帶C標記的方框表示一個類。方框中可添加成員變量和函數的說明。

成員變量的格式為：name:type

成員函數的格式為：name （parameter-list） ： return-type，每一個形參的格式為：name:type

除了在方框中定義成員變量，還可以使用箭頭指向類的方式來表示。所以，上圖展示出HAL類中有著各種驅動類的實例。

業務層在獲得了hal后，就可以通過它訪問具體的驅動接口，比如說：

使用hal.console-》printf（）打印日志

使用AP_HAL：：millis（）和AP_HAL：：micros（） 獲取上電啟動到現在所經過的時間

使用hal.scheduler-》delay（） 和 hal.scheduler-》delay_microseconds（） 睡眠一定的時間

使用hal.gpio-》pinMode（）， hal.gpio-》read（） 和 hal.gpio-》write（） 訪問GPIO引腳

通過hal.i2c_mgr訪問I2C設備

通過hal.spi訪問SPI設備

AP_HAL_PX4框架

libraries/AP_HAL之中定義的是HAL接口，順帶著介紹下其實現。

PX4的實現位于libraries/AP_HAL_PX4之中，其結構為：

定義了PX4命名空間，基本上所有驅動類位于其中。

每一個AP_HAL空間中的驅動類，都在PX4空間中能找到實現。比如PX4UARTDriver繼承自UARTDriver，實現了其定義的全部接口。

HAL_PX4類繼承自HAL類。

PX4中的驅動類依賴于PX4Firmware中間件以實現具體的驅動功能，比如PX4_I2C繼承自PX4Firmware中的I2C類。

下圖描述了AP_HAL空間，PX4空間以及PX4Firmware的關系。只繪制了少部分AP_HAL中的類以做示意。若全放出來的話，那這圖就沒法兒看了。

HAL_PX4的初始化詳見其構造函數，在HAL_PX4_Class.cpp之中

HAL_PX4：：HAL_PX4（） ：

AP_HAL：：HAL（

&uartADriver， /* uartA */

&uartBDriver， /* uartB */

&uartCDriver， /* uartC */

&uartDDriver， /* uartD */

&uartEDriver， /* uartE */

&uartFDriver， /* uartF */

&i2c_mgr_instance，

&spi_mgr_instance，

&analogIn， /* analogin */

&storageDriver， /* storage */

&uartADriver， /* console */

&gpioDriver， /* gpio */

&rcinDriver， /* rcinput */

&rcoutDriver， /* rcoutput */

&schedulerInstance， /* scheduler */

&utilInstance， /* util */

nullptr， /* no onboard optical flow */

nullptr） /* CAN */

{}

setup（）和loop（）

ArduCopter的主要工作，在setup（）和loop（）中執行。setup（）進行初始化，而loop（）如其名稱一樣，將被底層循環調用，以主持飛行工作的大局。

搞清聯系

至目前為止，我們了解了程序框架的4要素，它們是：

程序入口：AP_HAL_MAIN_CALLBACKS（&copter）

隨處可用的hal引用：const AP_HAL：：HAL& hal = AP_HAL：：get_HAL（）;

setup（）

loop（）

我們知道setup（）會被底層調用一次，還知道loop（）會被底層循環調用。不過它們是在哪兒被調用的呢？這與AP_HAL_MAIN_CALLBACKS定義出的入口函數有什么關聯嗎？如果不搞清楚這些問題，筆者覺得并不算是掌握了脈絡。好的，現在筆者來深入剖析它們的關系。

setup，loop和底層的關系是這樣的：

Callbacks接口定義了兩個純虛函數setup和loop。

ArduCopter.cpp中定義了Copter類，繼承Callbacks接口，所以Copter的實例就是Callbacks對象。

ArduCopter.cpp將創建出的Callbacks對象傳遞給HAL_PX4，以供其調用。就是ArduPilot_main中的那行代碼：

hal.run（argc， argv， &copter）;

具體的調用流程是這樣的：

AHRS_Test.cpp通過宏AP_HAL_MAIN定義出了Ardupilot_main（）。

上電啟動后，硬件平臺調用Ardupilot_main（）。

Ardupilot_main（）調用HAL層的入口函數HAL_PX4：：run（），并將自己的Callbacks對象傳遞給它。

HAL_PX4：：run（）調用了自己的main_loop（）。

main_loop先調用AHRS_Test.cpp的setup（）進行業務初始化。

main_loop循環調用AHRS_Test.cpp的loop（），直到斷電。

一圖以蔽之

也可以用eclipse的查看調用棧功能來展示：

看完上述內容，是不是感覺有點復雜呢。筆者來總結一下：

首先，記住兩點最關鍵的：

ArduCopter代碼定義setup（）和loop（），setup（）進行初始化，而運行時的主要工作是在loop（）之中。

HAL_PX4_Class負責程序調度，即調用setup（）和loop（）。

不過呢，有點繞的是：HAL_PX4_Class并不提供程序入口，程序入口由ArduCopter提供，它們會定義Ardupilot_main（）。由Ardupilot_main（）啟動HAL_PX4_Class。

為什么不由HAL_PX4_Class來定義程序入口？再想想？因為程序有很多可選入口嘛。還記得APMrover2，AntennaTracker，ArduPlane和ArduSub嗎。

深入淺出

我們了解了程序框架的4要素，并且深入研究了它們之間的調度邏輯，是時候“淺出”一下了。我們來看一個ardupilot里面的example。

為一覽全貌，筆者折疊了很多代碼塊。大家發現什么了沒？程序4要素都在其中。example是獨立于業務代碼之外，可單獨運行的代碼。其與業務代碼處在相同的層次上，它們都依賴于共享庫和HAL。

AHRS_Test.cpp是姿態解算的示例。ardupilot工程中有各種示例代碼，有AP_HAL驅動的example，也有各種共享庫的example，有兩個作用：

演示功能模塊的用法。

對功能模塊進行測試。當我們實現了新平臺的HAL接口時，使用example來驗證，遠比直接跑業務代碼要方便有效。

在工程根目錄下查找所有示例：

怎么用這個示例呢，使用如下命令編譯固件并上傳：

。/waf build --target examples/AHRS_Test --upload

ardupilot是單線程的嗎？

看了由setup（）和loop（）組成的框架，你可能會認為ardupilot是單線程的。早期的ardupilot跑在ardunio上，setup（）和loop（）就是歷史的痕跡。那時的ardupilot確實是loop外加一個定時器回調，算是2個線程。

從pixhawk開始，這一切就不同了。pixhawk平臺的底層系統是Nuttx實時操作系統，其支持帶優先級的多線程功能，而AP_HAL_PX4充分利用了這一特性。請看筆者嘔心瀝血繪制的高清大圖（請在新標簽頁中打開圖片）：

上圖展示了各個線程的大體框架，左邊線程的優先級大于右邊的。除了main_loop和timer_thread，還有許多其他的線程。最多的是總線通信線程，有SPI， I2C， CAN， Uart。也可通過下表進行了解，優先級的數值越大代表優先級越高。

名稱優先級定時間隔函數名說明

bus242

DeviceBus：：bus_thread用于SPI

timer1811msPX4Scheduler：：_timer_thread

main1802.5msmain_loop主線程

uavcan1791msPX4Scheduler：：_uavcan_thread

bus178

DeviceBus：：bus_thread用于I2C

uart601msPX4Scheduler：：_uart_thread

storage5910msPX4Scheduler：：_storage_thread

io581msPX4Scheduler：：_io_thread

main_loop里面的并行實際上是一個偽并行，由ardupilot自己設計的AP_Scheduler系統來進行調度。這里就不展開了，后續有機會的話會單獨介紹。

深入細節

不知不覺寫了好多，下一篇再講具體的HAL接口。對了，講解移植流程，就得等Ardupilot移植經驗分享（4）了。

原文標題：Ardupilot移植經驗分享（2）

文章出處：【微信公眾號：RTThread物聯網操作系統】歡迎添加關注！文章轉載請注明出處。

責任編輯：haq