接Ardupilot移植經驗分享（2）

深入細節

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

分析HAL接口的含義，包括功能，入參及返回值的具體含義。

分析HAL_PX4的實現，看看有沒有可借鑒之處。

調度接口

AP_HAL：：Scheduler提供了程序調度相關的接口。主要分為兩類：

延時函數

注冊回調

延時函數有3個，1個毫秒級延時，2個微秒級延時。這里的延時可不是死等，而是睡眠一段時間，在此期間讓出CPU的使用權以執行其他的線程。

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

大家可能會覺得，delay是最簡單的，delay_microseconds會比較復雜。因為通常來說，毫秒級延時是RTOS的基礎API，比如RT-Thread的rt_thread_mdelay。

rt_err_t rt_thread_mdelay（rt_int32_t ms）;

不過Scheduler：：delay除了簡單的睡眠延時，還多了一項任務。這就要先提到注冊回調接口中的一個：

typedef void（*Proc）（void）;

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

這接口的功能是：注冊一個延時回調。當某個線程調用delay進行延時時，若延時的時間大于min_time_ms，則delay函數將會調用這個延時回調。

這延時回調的意義是，當你睡眠的時候，正好可以執行某一個指定的任務，不要浪費時間。你可能會想，RTOS的延時本就會讓出CPU，本就會讓別的線程得以執行，何必多此一舉呢？我們看看是誰注冊了這個回調。

其中之一是mavlink模塊。

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

在這里插入圖片描述

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

我們再看看PX4的實現。延時的功能依賴于一個微秒級延時的接口delay_microseconds_semaphore，不過它每次只延時1000微秒，多余的時間會去執行延時回調。

微秒級延時delay_microseconds

筆者當初看到這接口時，有些頭疼。因為RT-Thread并沒有微秒級別的延時函數，再強調一遍，這不是死等，是要睡眠讓權的。

直接看PX4的實現：

void PX4Scheduler：：delay_microseconds（uint16_t usec）

{

perf_begin（_perf_delay）;

delay_microseconds_semaphore（usec）;

perf_end（_perf_delay）;

}

這是上節提到的delay_microseconds_semaphore的馬甲，脫了它：

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

調用hrt_call_after注冊一個定時器回調，定時時間是usec，單位微秒。回調函數是信號量發送函數sem_post，回調參數是信號量wait_semaphore。

使用sem_wait等待信號量wait_semaphore，此時當前線程會堵塞，進入睡眠狀態。

到達定時時間后，底層就執行這個回調，即sem_post（&wait_semaphore）。

sem_wait接收到了信號量wait_semaphore，該線程被喚醒。

結合時序圖來理解：

微秒級的延時函數，依賴于微秒級的定時回調，也就是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沒有相應功能而煩惱。關于定時器，RT-Thread有類似的功能，那就是rt_timer，不過這是毫秒級的。

rt_timer_t rt_timer_create（const char *name，

void （*timeout）（void *parameter），

void *parameter，

rt_tick_t time，

rt_uint8_t flag）

可能你會想，一般單片機里面不是都有硬件定時器嗎？實現微秒級的定時功能很簡單啊。確實簡單，也不簡單。因為hrt_call_after提供的定時功能是要支持并發的，千言萬語不如一圖：

至于pixhawk的實現以及筆者的移植，將專門出一篇文章來講解。

delay_microseconds_boost

這個同樣依賴于hrt_call_after，只是在睡眠的時候提高了優先級，以使得自己可在第一時間被喚醒。

注冊回調

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

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

相對來說，這兩接口就簡單的多了。它們用于注冊在timer線程和io線程中運行的回調函數。

注冊函數將回調指針加入到數組中。

在相應線程中，定時的一一執行。

那么，這線程是怎么創建的呢？PX4Scheduler的初始化函數中，會創建許多線程。

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”（可移植操作系統接口） 的縮寫，POSIX 是 IEEE Computer Society 為了提高不同操作系統的兼容性和應用程序的可移植性而制定的一套標準。

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

串口驅動

AP_HAL的串口驅動框架由4個類組成，層層分工明確。

Print的功能，如其名稱，負責打印輸出。write系列為最底層的字節輸出接口，需要由具體的HAL平臺實現。print和println系列提供打印功能。所謂的打印，比如打印float，就是以字符格式輸出float。筆者只列了幾個print接口，實際上遠比這個多。

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

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

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

在這里插入圖片描述

除了print系列函數由AP_HAL中的類實現，其他的功能，比如read， write， begin， flow_control，都要由具體的HAL平臺實現。PX4UARTDriver就是具體的實現類。

串口綁定

Ardupilot有6個串口，定義在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構造函數和set_device_path用于與具體的底層串口相綁定。HAL_PX4_Class.c中定義了綁定關系：

// 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的實現

大家可能認為串口的讀寫是非常簡單的操作。其實不然，Ardupilot作為一個對實時要求很高的飛控程序，在應用層調用串口讀寫函數時，是不允許堵塞的。這需要一些額外的工作來實現。

PX4UARTDriver使用接收緩存和發送緩存來實現異步讀寫。

write是將數據寫入到發送緩存_writebuf之中。_writebuf是一個隊列，實質上是環形數組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從接收緩存中提取數據，若沒有則返回-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;

}

將發送緩存的數據寫入串口和從串口接收數據以填充接收緩存的工作，在PX4UARTDriver：：_timer_tick函數中實現。而所有串口的_timer_tick由一個統一的串口線程來調度。

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;

}

看過前面高清大圖的，應該對這個有印象：

SPI和I2C驅動

我們再看兩個驅動，SPI驅動和I2C驅動。這兩個驅動有很多共同之處：

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

都有主從概念，每次傳輸由主機發起，由從機應答。

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

在這里插入圖片描述

UML圖示說明

上圖為UML類圖。前面提到類圖的語法，這里做一點補充。

變量和函數左邊有顏色的符號表示訪問權限，綠色圓圈是public，黃色菱形是protected。

斜體函數為純虛函數，需要由子類實現。

transfer

Ardupilot的I2C和SPI驅動主要是用于與傳感器通信，所以Device類提供了兩個常用的接口：read_register和write_register，并且實現了它們。當然，這是基于transfer接口實現的，而transfer交由子類來實現，畢竟SPI和I2C的實現是不同的。

/*

* 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實現的是先寫后讀。而對于SPI來說，transfer內部是同時讀寫。

SPIDevice和I2CDevice

它們添加了自身獨有的接口。

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

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

Periodic Callback

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

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

各傳感器驅動通過register_periodic_callback注冊定時回調，在回調之中訪問對應的傳感器。同一總線的所有定時回調是在同一個線程中被執行的，這就是串行化。

PX4在實現時，使用DeviceBus實現這個串行化的功能，其會為每一條總線創建一個線程。

其內部實現，無非是創建線程，將回調添加到一個鏈表之中。在函數中，POSIX接口的調用清晰可見。筆者的意思是，可以直接拿來用啦。

Manager

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

小結

SPI和I2C傳輸的具體實現，沒啥好說的。最值得說的，是Ardupilot抽象出了Device基類，為應用層提供串行化的訪問功能。而這串行化，是靠創建線程和回調鏈表來實現。

是時候放一張高清大圖了：

總結

到目前為止，我們看了調度接口，串口驅動，SPI和I2C驅動。調度接口中的微秒級延時接口非常關鍵，因為很多地方使用了它，并且它的實現有些困難。至于串口、SPI等驅動接口，只要我們理清了它們的層級關系，明確了各接口的作用，移植時不會有什么大問題。并且，這些驅動接口的實現，很多地方可以參考PX4的實現，甚至是直接復制過來用。

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

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

責任編輯：haq