關(guān)于Ardupilot移植的經(jīng)驗(yàn)分享

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

下面開始閱讀源碼之旅。剛才提到，ardupilot工程有700k行代碼，這個(gè)統(tǒng)計(jì)出版官方開發(fā)者wiki。

雖然龐大的體量令人望而生畏，但若明確了目標(biāo)，把握了脈絡(luò)，閱讀也非難事。下面讓筆者按著當(dāng)初的思路，分享閱讀的步驟和要點(diǎn)，帶領(lǐng)大家體驗(yàn)一次奇妙的閱讀之旅。

下面的內(nèi)容分為三個(gè)部分：

明確目標(biāo)

把握脈絡(luò)

深入細(xì)節(jié)

明確目標(biāo)

我們的最終目標(biāo)是通過底層適配的方法，移植ardupilot到自己的硬件平臺(tái)。不清楚底層適配的同學(xué)，請(qǐng)看Ardupilot移植經(jīng)驗(yàn)分享（1）的“移植Ardupilot的方法有兩種”部分。而閱讀源碼的目標(biāo)，也就非常簡單，即要搞清楚如何進(jìn)行適配：

要適配哪些接口（或者說函數(shù)）？

實(shí)現(xiàn)這些接口時(shí)，可參考哪些代碼？

會(huì)不會(huì)有難以實(shí)現(xiàn)的接口？

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

前篇文章介紹ardupilot時(shí)，提到代碼分為5個(gè)部分，筆者帶領(lǐng)大家回憶下最重要的3個(gè)：

載具代碼：業(yè)務(wù)層的代碼，比如飛行器初始化，飛行模式控制。

共享庫：libraries中除以AP_HAL開頭的所有子目錄。包含傳感器驅(qū)動(dòng)，姿態(tài)位置估算與控制。

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

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

UML圖示說明

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

上述是UML組件圖，非標(biāo)準(zhǔn)語法。

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

實(shí)線表示關(guān)聯(lián)。AP_HAL_PX4實(shí)現(xiàn)了AP_HAL接口，準(zhǔn)確說是繼承的關(guān)系。不過筆者在UML類圖中才見過繼承的語法，所以這里僅使用實(shí)線。

我們要實(shí)現(xiàn)的是AP_HAL接口，是上圖中的紅色圓圈?，F(xiàn)有的實(shí)現(xiàn)有AP_HAL_PX4（對(duì)應(yīng)Pixhawk平臺(tái)），AP_HAL_Linux（對(duì)應(yīng)NAVIO）。我們要添加一個(gè)平臺(tái)，為圖中藍(lán)色部分，即AP_HAL_TI。

閱讀的重點(diǎn)也就確定了：

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

參考AP_HAL_PX4的實(shí)現(xiàn)，位于libraires/AP_HAL_PX4。

不過若想完成移植大業(yè)，僅閱讀這兩塊內(nèi)容是不夠的。

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

AP_HAL_PX4依賴于底層平臺(tái)（PX4Firmware）所提供的功能。當(dāng)你想?yún)⒖糀P_HAL_PX4是如何實(shí)現(xiàn)某個(gè)接口的時(shí)候，你可能會(huì)發(fā)現(xiàn)它只是簡單地把任務(wù)交給了PX4Firmware。當(dāng)然，這只是少數(shù)情況。參考大部分的實(shí)現(xiàn)時(shí)，看AP_HAL_PX4本身的代碼就足夠了。

因此，除了重點(diǎn)閱讀AP_HAL和AP_HAL_PX4外，我們需要適當(dāng)?shù)南蛏虾拖蛳卵由臁Ｓ绕涫窍蛏希莆诊w控業(yè)務(wù)代碼的整體框架，在需要深入某個(gè)細(xì)節(jié)時(shí)，至少要知道去哪兒看。

把握脈絡(luò)

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

main函數(shù)在哪兒？

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

主函數(shù)的定義位于ArduCopter/ArduCopter.cpp的結(jié)尾處。

AP_HAL_MAIN_CALLBACKS（&copter）;

AP_HAL_MAIN_CALLBACKS用于定義主函數(shù)。

#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;

}

}

主函數(shù)的名稱不一定是大家熟悉的main，對(duì)于pixhawk平臺(tái)來說，其是Ardupilot_main。

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

#define AP_MAIN __EXPORT ArduPilot_main

#endif

經(jīng)過預(yù)編譯后，其最終的形態(tài)如下，這就是ardupilot的主函數(shù)。

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;

}

}

也許你會(huì)疑惑，為什么要如此大費(fèi)周章地定義主函數(shù)呢？

首先，主函數(shù)的名稱依平臺(tái)而定，這肯定是要用宏的。

其次，ardupilot項(xiàng)目不僅僅是ArduCopter，它還有APMrover2，AntennaTracker，ArduPlane和ArduSub，即ardupilot工程有多個(gè)入口函數(shù)，在編譯時(shí)根據(jù)目標(biāo)類型來選擇。使用AP_HAL_MAIN_CALLBACKS來定義，可避免重復(fù)代碼，并且后續(xù)修改時(shí)只需要修改AP_HAL_MAIN_CALLBACKS宏即可。

放一起看下效果：

ArduCopter/ArduCopter.cpp

AP_HAL_MAIN_CALLBACKS（&copter）;

ArduSub/ArduSub.cpp

AP_HAL_MAIN_CALLBACKS（&sub）;

如果你想知道筆者是如何找到這個(gè)入口的，請(qǐng)看尋找ardupilot的main函數(shù)。

HAL引用

ArduPilot_main中只有一行關(guān)鍵代碼：

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

hal是HAL實(shí)例的引用，這個(gè)引用定義在Copter.cpp中。

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

get_HAL（）函數(shù)為業(yè)務(wù)層提供獲取HAL實(shí)例的接口，其在AP_HAL_Namespace.h中聲明，由HAL_PX4實(shí)現(xiàn)。

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中實(shí)現(xiàn)了get_HAL函數(shù)，其創(chuàng)建并返回HAL實(shí)例。

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

static const HAL_PX4 hal_px4;

return hal_px4;

}

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

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

UML圖示說明

上圖為UML類圖。每一個(gè)帶C標(biāo)記的方框表示一個(gè)類。方框中可添加成員變量和函數(shù)的說明。

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

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

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

業(yè)務(wù)層在獲得了hal后，就可以通過它訪問具體的驅(qū)動(dòng)接口，比如說：

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

使用AP_HAL：：millis（）和AP_HAL：：micros（） 獲取上電啟動(dòng)到現(xiàn)在所經(jīng)過的時(shí)間

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

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

通過hal.i2c_mgr訪問I2C設(shè)備

通過hal.spi訪問SPI設(shè)備

AP_HAL_PX4框架

libraries/AP_HAL之中定義的是HAL接口，順帶著介紹下其實(shí)現(xiàn)。

PX4的實(shí)現(xiàn)位于libraries/AP_HAL_PX4之中，其結(jié)構(gòu)為：

定義了PX4命名空間，基本上所有驅(qū)動(dòng)類位于其中。

每一個(gè)AP_HAL空間中的驅(qū)動(dòng)類，都在PX4空間中能找到實(shí)現(xiàn)。比如PX4UARTDriver繼承自UARTDriver，實(shí)現(xiàn)了其定義的全部接口。

HAL_PX4類繼承自HAL類。

PX4中的驅(qū)動(dòng)類依賴于PX4Firmware中間件以實(shí)現(xiàn)具體的驅(qū)動(dòng)功能，比如PX4_I2C繼承自PX4Firmware中的I2C類。

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

HAL_PX4的初始化詳見其構(gòu)造函數(shù)，在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（）中執(zhí)行。setup（）進(jìn)行初始化，而loop（）如其名稱一樣，將被底層循環(huán)調(diào)用，以主持飛行工作的大局。

搞清聯(lián)系

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

程序入口：AP_HAL_MAIN_CALLBACKS（&copter）

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

setup（）

loop（）

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

setup，loop和底層的關(guān)系是這樣的：

Callbacks接口定義了兩個(gè)純虛函數(shù)setup和loop。

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

ArduCopter.cpp將創(chuàng)建出的Callbacks對(duì)象傳遞給HAL_PX4，以供其調(diào)用。就是ArduPilot_main中的那行代碼：

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

具體的調(diào)用流程是這樣的：

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

上電啟動(dòng)后，硬件平臺(tái)調(diào)用Ardupilot_main（）。

Ardupilot_main（）調(diào)用HAL層的入口函數(shù)HAL_PX4：：run（），并將自己的Callbacks對(duì)象傳遞給它。

HAL_PX4：：run（）調(diào)用了自己的main_loop（）。

main_loop先調(diào)用AHRS_Test.cpp的setup（）進(jìn)行業(yè)務(wù)初始化。

main_loop循環(huán)調(diào)用AHRS_Test.cpp的loop（），直到斷電。

一圖以蔽之

也可以用eclipse的查看調(diào)用棧功能來展示：

看完上述內(nèi)容，是不是感覺有點(diǎn)復(fù)雜呢。筆者來總結(jié)一下：

首先，記住兩點(diǎn)最關(guān)鍵的：

ArduCopter代碼定義setup（）和loop（），setup（）進(jìn)行初始化，而運(yùn)行時(shí)的主要工作是在loop（）之中。

HAL_PX4_Class負(fù)責(zé)程序調(diào)度，即調(diào)用setup（）和loop（）。

不過呢，有點(diǎn)繞的是：HAL_PX4_Class并不提供程序入口，程序入口由ArduCopter提供，它們會(huì)定義Ardupilot_main（）。由Ardupilot_main（）啟動(dòng)HAL_PX4_Class。

為什么不由HAL_PX4_Class來定義程序入口？再想想？因?yàn)槌绦蛴泻芏嗫蛇x入口嘛。還記得APMrover2，AntennaTracker，ArduPlane和ArduSub嗎。

深入淺出

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

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

AHRS_Test.cpp是姿態(tài)解算的示例。ardupilot工程中有各種示例代碼，有AP_HAL驅(qū)動(dòng)的example，也有各種共享庫的example，有兩個(gè)作用：

演示功能模塊的用法。

對(duì)功能模塊進(jìn)行測(cè)試。當(dāng)我們實(shí)現(xiàn)了新平臺(tái)的HAL接口時(shí)，使用example來驗(yàn)證，遠(yuǎn)比直接跑業(yè)務(wù)代碼要方便有效。

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

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

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

ardupilot是單線程的嗎？

看了由setup（）和loop（）組成的框架，你可能會(huì)認(rèn)為ardupilot是單線程的。早期的ardupilot跑在ardunio上，setup（）和loop（）就是歷史的痕跡。那時(shí)的ardupilot確實(shí)是loop外加一個(gè)定時(shí)器回調(diào)，算是2個(gè)線程。

從pixhawk開始，這一切就不同了。pixhawk平臺(tái)的底層系統(tǒng)是Nuttx實(shí)時(shí)操作系統(tǒng)，其支持帶優(yōu)先級(jí)的多線程功能，而AP_HAL_PX4充分利用了這一特性。請(qǐng)看筆者嘔心瀝血繪制的高清大圖（請(qǐng)?jiān)谛?a target="_blank">標(biāo)簽頁中打開圖片）：

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

名稱優(yōu)先級(jí)定時(shí)間隔函數(shù)名說明

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里面的并行實(shí)際上是一個(gè)偽并行，由ardupilot自己設(shè)計(jì)的AP_Scheduler系統(tǒng)來進(jìn)行調(diào)度。這里就不展開了，后續(xù)有機(jī)會(huì)的話會(huì)單獨(dú)介紹。

深入細(xì)節(jié)

不知不覺寫了好多，下一篇再講具體的HAL接口。對(duì)了，講解移植流程，就得等Ardupilot移植經(jīng)驗(yàn)分享（4）了。

原文標(biāo)題：Ardupilot移植經(jīng)驗(yàn)分享（2）

文章出處：【微信公眾號(hào)：RTThread物聯(lián)網(wǎng)操作系統(tǒng)】歡迎添加關(guān)注！文章轉(zhuǎn)載請(qǐng)注明出處。

責(zé)任編輯：haq