Ardupilot移植的深入細(xì)節(jié)分享

接Ardupilot移植經(jīng)驗(yàn)分享（2）

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

是時(shí)候深入具體的HAL接口了。筆者并不打算一一講解所有的接口，而是挑選一些有代表性的來分析，主要的內(nèi)容是：

分析HAL接口的含義，包括功能，入?yún)⒓胺祷刂档木唧w含義。

分析HAL_PX4的實(shí)現(xiàn)，看看有沒有可借鑒之處。

調(diào)度接口

AP_HAL：：Scheduler提供了程序調(diào)度相關(guān)的接口。主要分為兩類：

延時(shí)函數(shù)

注冊回調(diào)

延時(shí)函數(shù)有3個(gè)，1個(gè)毫秒級(jí)延時(shí)，2個(gè)微秒級(jí)延時(shí)。這里的延時(shí)可不是死等，而是睡眠一段時(shí)間，在此期間讓出CPU的使用權(quán)以執(zhí)行其他的線程。

virtual void delay（uint16_t ms） = 0;

virtual void delay_microseconds（uint16_t us） = 0;

virtual void delay_microseconds_boost（uint16_t us） { delay_microseconds（us）; }

看似簡單的delay

大家可能會(huì)覺得，delay是最簡單的，delay_microseconds會(huì)比較復(fù)雜。因?yàn)橥ǔ碚f，毫秒級(jí)延時(shí)是RTOS的基礎(chǔ)API，比如RT-Thread的rt_thread_mdelay。

rt_err_t rt_thread_mdelay（rt_int32_t ms）;

不過Scheduler：：delay除了簡單的睡眠延時(shí)，還多了一項(xiàng)任務(wù)。這就要先提到注冊回調(diào)接口中的一個(gè)：

typedef void（*Proc）（void）;

virtual void register_delay_callback（AP_HAL：：Proc， uint16_t min_time_ms） = 0;

這接口的功能是：注冊一個(gè)延時(shí)回調(diào)。當(dāng)某個(gè)線程調(diào)用delay進(jìn)行延時(shí)時(shí)，若延時(shí)的時(shí)間大于min_time_ms，則delay函數(shù)將會(huì)調(diào)用這個(gè)延時(shí)回調(diào)。

這延時(shí)回調(diào)的意義是，當(dāng)你睡眠的時(shí)候，正好可以執(zhí)行某一個(gè)指定的任務(wù)，不要浪費(fèi)時(shí)間。你可能會(huì)想，RTOS的延時(shí)本就會(huì)讓出CPU，本就會(huì)讓別的線程得以執(zhí)行，何必多此一舉呢？我們看看是誰注冊了這個(gè)回調(diào)。

其中之一是mavlink模塊。

hal.scheduler-》register_delay_callback（mavlink_delay_cb_static， 5）;

在這里插入圖片描述

注釋里面說明了原因，這是為了在長時(shí)間的初始化函數(shù)（setup）中能進(jìn)行MAVLink交互。主要的MAVLink任務(wù)是在loop（）中的AP_Scheduler調(diào)度系統(tǒng)中執(zhí)行，就是紅圈部分（不過紅圈里面沒提到MAVLink，不要介意哈）。setup（）是順序執(zhí)行一系列的初始化函數(shù)，想在它里面去進(jìn)行MAVLink任務(wù)，就靠這register_delay_callback了。

我們再看看PX4的實(shí)現(xiàn)。延時(shí)的功能依賴于一個(gè)微秒級(jí)延時(shí)的接口delay_microseconds_semaphore，不過它每次只延時(shí)1000微秒，多余的時(shí)間會(huì)去執(zhí)行延時(shí)回調(diào)。

微秒級(jí)延時(shí)delay_microseconds

筆者當(dāng)初看到這接口時(shí)，有些頭疼。因?yàn)镽T-Thread并沒有微秒級(jí)別的延時(shí)函數(shù)，再強(qiáng)調(diào)一遍，這不是死等，是要睡眠讓權(quán)的。

直接看PX4的實(shí)現(xiàn)：

void PX4Scheduler：：delay_microseconds（uint16_t usec）

{

perf_begin（_perf_delay）;

delay_microseconds_semaphore（usec）;

perf_end（_perf_delay）;

}

這是上節(jié)提到的delay_microseconds_semaphore的馬甲，脫了它：

該函數(shù)使用hrt_call_after和信號(hào)量來完成微秒級(jí)睡眠的功能。hrt是High-resolution timer的縮寫，高精度定時(shí)器。

調(diào)用hrt_call_after注冊一個(gè)定時(shí)器回調(diào)，定時(shí)時(shí)間是usec，單位微秒?；卣{(diào)函數(shù)是信號(hào)量發(fā)送函數(shù)sem_post，回調(diào)參數(shù)是信號(hào)量wait_semaphore。

使用sem_wait等待信號(hào)量wait_semaphore，此時(shí)當(dāng)前線程會(huì)堵塞，進(jìn)入睡眠狀態(tài)。

到達(dá)定時(shí)時(shí)間后，底層就執(zhí)行這個(gè)回調(diào)，即sem_post（&wait_semaphore）。

sem_wait接收到了信號(hào)量wait_semaphore，該線程被喚醒。

結(jié)合時(shí)序圖來理解：

微秒級(jí)的延時(shí)函數(shù)，依賴于微秒級(jí)的定時(shí)回調(diào)，也就是hrt_call_after：

/**

* Call callout（arg） after delay has elapsed.

*

* If callout is NULL， this can be used to implement a timeout by testing the call

* with hrt_called（）。

*/

__EXPORT extern void hrt_call_after（struct hrt_call *entry， hrt_abstime delay， hrt_callout callout， void *arg）;

hrt_call_after屬于pixhawk底層的接口。筆者一度以為是Nuttx提供的功能，并且為RT-Thread沒有相應(yīng)功能而煩惱。關(guān)于定時(shí)器，RT-Thread有類似的功能，那就是rt_timer，不過這是毫秒級(jí)的。

rt_timer_t rt_timer_create（const char *name，

void （*timeout）（void *parameter），

void *parameter，

rt_tick_t time，

rt_uint8_t flag）

可能你會(huì)想，一般單片機(jī)里面不是都有硬件定時(shí)器嗎？實(shí)現(xiàn)微秒級(jí)的定時(shí)功能很簡單啊。確實(shí)簡單，也不簡單。因?yàn)閔rt_call_after提供的定時(shí)功能是要支持并發(fā)的，千言萬語不如一圖：

至于pixhawk的實(shí)現(xiàn)以及筆者的移植，將專門出一篇文章來講解。

delay_microseconds_boost

這個(gè)同樣依賴于hrt_call_after，只是在睡眠的時(shí)候提高了優(yōu)先級(jí)，以使得自己可在第一時(shí)間被喚醒。

注冊回調(diào)

virtual void register_timer_process（AP_HAL：：MemberProc） = 0;

virtual void register_io_process（AP_HAL：：MemberProc） = 0;

相對來說，這兩接口就簡單的多了。它們用于注冊在timer線程和io線程中運(yùn)行的回調(diào)函數(shù)。

注冊函數(shù)將回調(diào)指針加入到數(shù)組中。

在相應(yīng)線程中，定時(shí)的一一執(zhí)行。

那么，這線程是怎么創(chuàng)建的呢？PX4Scheduler的初始化函數(shù)中，會(huì)創(chuàng)建許多線程。

void PX4Scheduler：：init（）

{

_main_task_pid = getpid（）;

// setup the timer thread - this will call tasks at 1kHz

pthread_attr_t thread_attr;

struct sched_param param;

pthread_attr_init（&thread_attr）;

pthread_attr_setstacksize（&thread_attr， 2048）;

param.sched_priority = APM_TIMER_PRIORITY;

（void）pthread_attr_setschedparam（&thread_attr， ?m）;

pthread_attr_setschedpolicy（&thread_attr， SCHED_FIFO）;

pthread_create（&_timer_thread_ctx， &thread_attr， &PX4Scheduler：：_timer_thread， this）;

// the UART thread runs at a medium priority

pthread_attr_init（&thread_attr）;

pthread_attr_setstacksize（&thread_attr， 2048）;

param.sched_priority = APM_UART_PRIORITY;

（void）pthread_attr_setschedparam（&thread_attr， ?m）;

pthread_attr_setschedpolicy（&thread_attr， SCHED_FIFO）;

pthread_create（&_uart_thread_ctx， &thread_attr， &PX4Scheduler：：_uart_thread， this）;

// the IO thread runs at lower priority

pthread_attr_init（&thread_attr）;

pthread_attr_setstacksize（&thread_attr， 2048）;

param.sched_priority = APM_IO_PRIORITY;

（void）pthread_attr_setschedparam（&thread_attr， ?m）;

pthread_attr_setschedpolicy（&thread_attr， SCHED_FIFO）;

pthread_create（&_io_thread_ctx， &thread_attr， &PX4Scheduler：：_io_thread， this）;

// the storage thread runs at just above IO priority

pthread_attr_init（&thread_attr）;

pthread_attr_setstacksize（&thread_attr， 1024）;

param.sched_priority = APM_STORAGE_PRIORITY;

（void）pthread_attr_setschedparam（&thread_attr， ?m）;

pthread_attr_setschedpolicy（&thread_attr， SCHED_FIFO）;

pthread_create（&_storage_thread_ctx， &thread_attr， &PX4Scheduler：：_storage_thread， this）;

}

pthread_attr_init，pthread_create，這些是POSIX的線程接口，由Nuttx提供。POSIX 是 “Portable Operating System Interface”（可移植操作系統(tǒng)接口） 的縮寫，POSIX 是 IEEE Computer Society 為了提高不同操作系統(tǒng)的兼容性和應(yīng)用程序的可移植性而制定的一套標(biāo)準(zhǔn)。

好消息是，RT-Thread從3.0版本開始提供POSIX接口。所以，當(dāng)我們移植的時(shí)候，很多地方可以參考AP_HAL_PX4的代碼，甚至是直接復(fù)制它的代碼。

串口驅(qū)動(dòng)

AP_HAL的串口驅(qū)動(dòng)框架由4個(gè)類組成，層層分工明確。

Print的功能，如其名稱，負(fù)責(zé)打印輸出。write系列為最底層的字節(jié)輸出接口，需要由具體的HAL平臺(tái)實(shí)現(xiàn)。print和println系列提供打印功能。所謂的打印，比如打印float，就是以字符格式輸出float。筆者只列了幾個(gè)print接口，實(shí)際上遠(yuǎn)比這個(gè)多。

Stream定義了輸入接口，available返回接收緩存中的字節(jié)數(shù)，read用于讀取輸入。

BetterStream添加了格式化輸出的接口，即大家熟悉的printf系列。

UARTDriver引入了與串口相關(guān)的接口，begin用于配置波特率、輸入輸出緩存，set_flow_control和get_flow_control與流控相關(guān)。

在這里插入圖片描述

除了print系列函數(shù)由AP_HAL中的類實(shí)現(xiàn)，其他的功能，比如read， write， begin， flow_control，都要由具體的HAL平臺(tái)實(shí)現(xiàn)。PX4UARTDriver就是具體的實(shí)現(xiàn)類。

串口綁定

Ardupilot有6個(gè)串口，定義在AP_HAL：：HAL之中。

AP_HAL：：UARTDriver* uartA;

AP_HAL：：UARTDriver* uartB;

AP_HAL：：UARTDriver* uartC;

AP_HAL：：UARTDriver* uartD;

AP_HAL：：UARTDriver* uartE;

AP_HAL：：UARTDriver* uartF;

PX4UARTDriver構(gòu)造函數(shù)和set_device_path用于與具體的底層串口相綁定。HAL_PX4_Class.c中定義了綁定關(guān)系：

// 3 UART drivers， for GPS plus two mavlink-enabled devices

static PX4UARTDriver uartADriver（UARTA_DEFAULT_DEVICE， “APM_uartA”）;

static PX4UARTDriver uartBDriver（UARTB_DEFAULT_DEVICE， “APM_uartB”）;

static PX4UARTDriver uartCDriver（UARTC_DEFAULT_DEVICE， “APM_uartC”）;

static PX4UARTDriver uartDDriver（UARTD_DEFAULT_DEVICE， “APM_uartD”）;

static PX4UARTDriver uartEDriver（UARTE_DEFAULT_DEVICE， “APM_uartE”）;

static PX4UARTDriver uartFDriver（UARTF_DEFAULT_DEVICE， “APM_uartF”）;

read和write的實(shí)現(xiàn)

大家可能認(rèn)為串口的讀寫是非常簡單的操作。其實(shí)不然，Ardupilot作為一個(gè)對實(shí)時(shí)要求很高的飛控程序，在應(yīng)用層調(diào)用串口讀寫函數(shù)時(shí)，是不允許堵塞的。這需要一些額外的工作來實(shí)現(xiàn)。

PX4UARTDriver使用接收緩存和發(fā)送緩存來實(shí)現(xiàn)異步讀寫。

write是將數(shù)據(jù)寫入到發(fā)送緩存_writebuf之中。_writebuf是一個(gè)隊(duì)列，實(shí)質(zhì)上是環(huán)形數(shù)組RingBuffer。

/*

write one byte to the buffer

*/

size_t PX4UARTDriver：：write（uint8_t c）

{

if （_uart_owner_pid ！= getpid（））{

return 0;

}

if （！_initialised） {

try_initialise（）;

return 0;

}

while （_writebuf.space（） == 0） {

if （_nonblocking_writes） {

return 0;

}

hal.scheduler-》delay（1）;

}

return _writebuf.write（&c， 1）;

}

read從接收緩存中提取數(shù)據(jù)，若沒有則返回-1。

/*

read one byte from the read buffer

*/

int16_t PX4UARTDriver：：read（）

{

if （_uart_owner_pid ！= getpid（））{

return -1;

}

if （！_initialised） {

try_initialise（）;

return -1;

}

uint8_t byte;

if （！_readbuf.read_byte（&byte）） {

return -1;

}

return byte;

}

將發(fā)送緩存的數(shù)據(jù)寫入串口和從串口接收數(shù)據(jù)以填充接收緩存的工作，在PX4UARTDriver：：_timer_tick函數(shù)中實(shí)現(xiàn)。而所有串口的_timer_tick由一個(gè)統(tǒng)一的串口線程來調(diào)度。

void *PX4Scheduler：：_uart_thread（void *arg）

{

PX4Scheduler *sched = （PX4Scheduler *）arg;

pthread_setname_np（pthread_self（）， “apm_uart”）;

while （！sched-》_hal_initialized） {

poll（nullptr， 0， 1）;

}

while （！_px4_thread_should_exit） {

sched-》delay_microseconds_semaphore（1000）;

// process any pending serial bytes

（（PX4UARTDriver *）hal.uartA）-》_timer_tick（）;

（（PX4UARTDriver *）hal.uartB）-》_timer_tick（）;

（（PX4UARTDriver *）hal.uartC）-》_timer_tick（）;

（（PX4UARTDriver *）hal.uartD）-》_timer_tick（）;

（（PX4UARTDriver *）hal.uartE）-》_timer_tick（）;

（（PX4UARTDriver *）hal.uartF）-》_timer_tick（）;

}

return nullptr;

}

看過前面高清大圖的，應(yīng)該對這個(gè)有印象：

SPI和I2C驅(qū)動(dòng)

我們再看兩個(gè)驅(qū)動(dòng)，SPI驅(qū)動(dòng)和I2C驅(qū)動(dòng)。這兩個(gè)驅(qū)動(dòng)有很多共同之處：

都有總線的概念，一條總線掛接許多設(shè)備。

都有主從概念，每次傳輸由主機(jī)發(fā)起，由從機(jī)應(yīng)答。

正是由于它們非常相似，Ardupilot提取出它們的共同之處，抽象成一個(gè)基類AP_HAL：：Device。下圖是Device的類圖，并非包含其所有內(nèi)容，僅列出了一些重要的元素。

在這里插入圖片描述

UML圖示說明

上圖為UML類圖。前面提到類圖的語法，這里做一點(diǎn)補(bǔ)充。

變量和函數(shù)左邊有顏色的符號(hào)表示訪問權(quán)限，綠色圓圈是public，黃色菱形是protected。

斜體函數(shù)為純虛函數(shù)，需要由子類實(shí)現(xiàn)。

transfer

Ardupilot的I2C和SPI驅(qū)動(dòng)主要是用于與傳感器通信，所以Device類提供了兩個(gè)常用的接口：read_register和write_register，并且實(shí)現(xiàn)了它們。當(dāng)然，這是基于transfer接口實(shí)現(xiàn)的，而transfer交由子類來實(shí)現(xiàn)，畢竟SPI和I2C的實(shí)現(xiàn)是不同的。

/*

* Core transfer function. This does a single bus transaction which

* sends send_len bytes and receives recv_len bytes back from the slave.

*

* Return： true on a successful transfer， false on failure.

*/

virtual bool transfer（const uint8_t *send， uint32_t send_len，

uint8_t *recv， uint32_t recv_len） = 0;

對于I2C來說，transfer實(shí)現(xiàn)的是先寫后讀。而對于SPI來說，transfer內(nèi)部是同時(shí)讀寫。

SPIDevice和I2CDevice

它們添加了自身獨(dú)有的接口。

SPIDevice添加了全雙工的傳輸接口transfer_fullduplex，與transfer接口所不同之處在于發(fā)送和接收緩存的長度一致。

I2CDevice中，set_address用于設(shè)置地址，set_split_transfers指定在先寫后讀的中間是否傳輸停止位。

Periodic Callback

Device的功能遠(yuǎn)不只是為SPI和I2C定義了統(tǒng)一的transfer接口。最重要的，是實(shí)現(xiàn)了應(yīng)用層訪問總線的串行化。SPI和I2C都是由一條總線掛接許多設(shè)備，無論是SPI或I2C，都不允許在同一時(shí)刻訪問多個(gè)設(shè)備。因此，Device提供了get_semaphore接口，以鎖定總線。當(dāng)然，這并不算是串行化，真正的串行化，是通過register_periodic_callback來實(shí)現(xiàn)。

virtual PeriodicHandle register_periodic_callback（uint32_t period_usec， PeriodicCb） = 0;

各傳感器驅(qū)動(dòng)通過register_periodic_callback注冊定時(shí)回調(diào)，在回調(diào)之中訪問對應(yīng)的傳感器。同一總線的所有定時(shí)回調(diào)是在同一個(gè)線程中被執(zhí)行的，這就是串行化。

PX4在實(shí)現(xiàn)時(shí)，使用DeviceBus實(shí)現(xiàn)這個(gè)串行化的功能，其會(huì)為每一條總線創(chuàng)建一個(gè)線程。

其內(nèi)部實(shí)現(xiàn)，無非是創(chuàng)建線程，將回調(diào)添加到一個(gè)鏈表之中。在函數(shù)中，POSIX接口的調(diào)用清晰可見。筆者的意思是，可以直接拿來用啦。

Manager

應(yīng)用層通過Device來訪問I2C和SPI設(shè)備，那么Device對象是哪來的呢？由I2CDeviceManager和SPIDeviceManager提供，而這兩個(gè)Manager的實(shí)例可通過HAL引用訪問。

小結(jié)

SPI和I2C傳輸?shù)木唧w實(shí)現(xiàn)，沒啥好說的。最值得說的，是Ardupilot抽象出了Device基類，為應(yīng)用層提供串行化的訪問功能。而這串行化，是靠創(chuàng)建線程和回調(diào)鏈表來實(shí)現(xiàn)。

是時(shí)候放一張高清大圖了：

總結(jié)

到目前為止，我們看了調(diào)度接口，串口驅(qū)動(dòng)，SPI和I2C驅(qū)動(dòng)。調(diào)度接口中的微秒級(jí)延時(shí)接口非常關(guān)鍵，因?yàn)楹芏嗟胤绞褂昧怂?，并且它的?shí)現(xiàn)有些困難。至于串口、SPI等驅(qū)動(dòng)接口，只要我們理清了它們的層級(jí)關(guān)系，明確了各接口的作用，移植時(shí)不會(huì)有什么大問題。并且，這些驅(qū)動(dòng)接口的實(shí)現(xiàn)，很多地方可以參考PX4的實(shí)現(xiàn)，甚至是直接復(fù)制過來用。

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

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

責(zé)任編輯：haq