前段時間導師叫我做撲翼無人機，工程上需要實現的，能夠通過程控飛起來，感覺難度挺大。先從研究PX4開始，打算一步步理解透整個PX4的框架，機型的適配、旋翼、固定翼的姿態控制，新機型的新增等等。不知道能不能做成，這裡先立個flag吧。

    這篇文件是我課程作業的一個報告，包含4個方面，硬體架構分析，Linux開發環境搭建，軟體架構分析，原始碼分析等。由於原始碼比較龐雜，這裡簡要分析下飛機的返航控制(RTL)，旋翼的混控器，以及uORB的使用三個方面。

Pixhawk是一個獨立的專案，旨在以低成本和高可用性為學術界，業餘愛好界和工業界提供高階行業標準自動駕駛儀硬體。由於其強大且豐富的硬體而廣受無人機愛好者的好評。Pixhawk支援APM和PX4的韌體，這裡我們對原生的PX4韌體做軟體架構分析。對於Pixhawk的系統架構分析我將從四方面來展開，分別是：硬體架構介紹、PX4開發環境搭建、PX4軟體架構分析及總結。這裡我們主要對軟體架構展開分析，根據架構的層次性對PX4中的導航模組、Mixer模組和uORB模組做分析和介紹。

關鍵詞：Pixhawk、PX4、硬體架構、軟體架構、uORB

1硬體架構分析

    Pixhawk是一款基於ARM晶片的32位開源飛控，由ETH的computer vision and geometry group的博士生Lorenz Meier開發。最初採用的是分體式的設計即px4（由px4fmu和px4io兩個元件組成），後合併成一個整體形成現在的pixhawk。其硬體和軟體都開源，因此衍生出很多不同的軟硬體版本，最初的分銷商是美國的3D Robotics。PIXHAWK是一款高效能自動駕駛儀，適用於固定翼，多旋翼，直升機，汽車，船隻以及任何可移動的其他機器人平臺。它針對高階研究，業餘和行業需求，並結合了PX4FMU + PX4IO的功能。Pixhawk的外形如圖1所示：

圖1 Pixhawk外形及介面

    Pixhawk採用帶有FPU的32位STM32F427晶片，它的採用Cortex M4核心，主主頻為168M，具有252MIPS的運算能力、256KB的RAM以及2MB的快閃記憶體，同時具有一片型號為STM32F103的故障協處理器晶片。在感測器的選用上面，它選用ST公司的MicroL3GD20H 16位陀螺儀和MicroLSM303D 14位加速度計/磁力計，選用Invensense公司MPU 6000 3軸加速度計/陀螺儀，採用MEAS公司的MS5611氣壓計。同時擁有非常豐富的硬體介面，它擁有5個UART（串列埠），其中一個具有高功率，2個帶HW流量控制，2個CAN（一個帶內部3.3V收發器，一個帶擴充套件聯結器）介面，Spektrum DSM / DSM2 / DSM-X衛星相容輸入介面，FutabaS.BUS相容輸入和輸出介面，PPM和訊號輸入介面，RSSI（PWM或電壓）輸入介面，同時還有I2C、SPI、ADC、內部和外部microUSB介面等等。其豐富的介面可以滿足任何可移動的機器人平臺的需要。在電力系統與保護方面，Pixhawk具有自動故障轉移功能的理想二極體，能夠適應大功率的伺服電機（最大10V）和大電流（10A +），所有外圍輸出均過流保護，所有輸入均受ESD保護。Pixhawk提供三個電源，它可以在電源上實現三重冗餘。三個電源分別是：電源模組輸入，伺服電機輸入，USB輸入。它優先使用電源模組的輸入，電壓範圍為4.8V到5.4V，其次是伺服電機的電壓輸入，電壓範圍也為4.8V到5.4V，最後選擇USB的電源輸入，電壓範圍也是一樣的。Pixhawk的原理圖可以從https://raw.githubusercontent.com/PX4/Hardware/master/FMUv2/PX4FMUv2.4.5.pdf處下載。

Pixhawk的硬體架構整體框圖如圖2所示：

圖2 硬體框架

    由圖中的內容可以看出硬體的大致框架，STM32F427晶片作為主控器承擔著全部感測器的資料讀寫，姿態的結算及控制，以及其他資訊的處理和控制。電源系統不僅為電機電調提供電源，同時還承擔著主控制器器和所有模組的電源供應。對於其它外圍系統以及和主控器的通訊方式分別如下所示：動力系統由主控器的四路PWM控制，除錯系統通過microUSB介面和主控器互動，檔案系統通過SDIO協議和主控制器互動，數傳系統通過433M模組和主控器互動，遙控系統通過SPI協議和主控制器互動，導航系統通過RTMC協議和主控器互動，定高及姿態控制系統都是通過I2C協議和主控器互動。這裡比較故障協處理器比較有趣，當飛機的主控制器跑飛或者發生其他錯誤導致無法對電機進行控制時，可以由協控制器感知到，進而獲得電機的控制權，將電機控制在安全的模式下，直到檢測到主控器對電機的控制恢復正常。協控制器不定時的讀取主控器的DMA計數暫存器，當這次計數暫存器的值和上去讀取到的值一致時說明主控器沒有對電機的PWM進行更新，主控制器失去了對電機的控制，這時協控制器得知主控制器異常，開始接管對電機的控制，直到DMA計數器更新，主控制器恢復對電機的控制。

2 PX4開發環境搭建

    PX4程式碼可以在 Mac OS,Linux 或者 Windows上進行開發，建議在Mac OS和Linux上進行開發，因為影像處理和高階導航在windows上不容易開發。如果不確定，新的開發者應預設用Linux和當前Ubuntu長期支援版本（Ubuntu LTS edition）。如果對Docker熟悉，你可以使用其中一個容器來安裝開發環境。

    我們使用Debian /Ubuntu LTS 作為Linux的標準支援版本，在這裡我的Linux版本為Ubuntu 16.04。

2.1 許可權設定

    為了便於開發，我們需要把使用者新增到使用者組”dialout”中，執行：

sudo usermod -a -G dialout $USER

    然後登出後，重新登入，因為重新登入後所做的改變才會有效。注意：永遠不要使用sudo來修復許可權問題，否則會帶來更多的許可權問題，需要重灌系統來解決。

2.2 安裝PX4的依賴包

    更新包列表，安裝下面編譯PX4的依賴包。先安裝cmake，執行：

sudo add-apt-repository ppa:george-edison55/cmake-3.x -y

sudo apt-get update

    安裝必備的軟體，比如python、git、qtcreator等等：

# 必備軟體

sudo apt-get install python-argparse git-core wget zip \

    python-empy qtcreatorcmake build-essential genromfs -y

安裝一些模擬工具：

# 模擬工具

sudo add-apt-repository ppa:openjdk-r/ppa

sudo apt-get update

sudo apt-get install openjdk-8-jre

sudo apt-get install ant protobuf-compiler libeigen3-devlibopencv-dev openjdk-8-jdk openjdk-8-jre clang-3.5 lldb-3.5 -y

2.3 安裝交叉編譯工具鏈

Ubuntu配備了一系列代理管理，這會嚴重干擾任何機器人相關的串列埠（或usb串列埠），解除安裝掉它也不會有什麼影響，我們這裡把它解除安裝：

sudo apt-get remove modemmanager

更新包列表和安裝下面的依賴包：

sudo apt-get install python-serial openocd \

    flex bison libncurses5-devautoconf texinfo build-essential \

    libftdi-dev libtoolzlib1g-dev \

    python-empy  -y

安裝arm-none-eabi編譯工具鏈，在新增arm-none-eabi工具鏈之前，請確保刪除殘餘：

sudo apt-get remove gcc-arm-none-eabi gdb-arm-none-eabibinutils-arm-none-eabi gcc-arm-embedded

sudo add-apt-repository --remove ppa:team-gcc-arm-embedded/ppa

如果需要在樹莓派上開發則需要安裝樹莓派上對於的工具鏈。樹莓派開發者應該從下面地址下載樹莓派Linux工具鏈。安裝指令碼會自動安裝交叉編譯工具鏈：

git clone https://github.com/pixhawk/rpi_toolchain.git

cd rpi_toolchain

./install_cross.sh

在工具鏈安裝過程中需要輸入密碼。如果不想把工具鏈安裝在預設位置/opt/rpi_toolchain，可以執行./install_cross.sh<PATH>向安裝指令碼傳入其它地址。安裝指令碼會自動配置需要的環境變數。最後，執行以下命令更新環境變數：

source ~/.profile

2.4 程式碼編譯

    開發環境搭建好之後，就可以開始下載程式碼並編譯了，PX4可以在控制檯或者圖形介面/IDE開發。在這裡我們對控制檯的開發進行簡要介紹。

    先建立工作目錄，然後從git上下載程式碼：

mkdir -p ~/src

cd ~/src

git clone https://github.com/PX4/Firmware.git

cd Firmware

git submodule update --init --recursive

cd ..

    現在可以通過編譯原始碼來構建二進位制檔案。在直接使用硬體前，推薦先進行模擬。然後就可以對程式碼進行編譯並下載了：

cd Firmware

make px4fmu-v2_default

    注意到“make”是一個字元命令編譯工具，“px4fmu-v2”是硬體/ardupilot版本，“default”是預設配置，所有的PX4編譯目標遵循這個規則。通過在命令後面新增‘upload’，編譯的二進位制程式就會通過USB上傳到飛控硬體。下載成功後的情況如下所示：

    當然這樣編譯後的程式碼是無法直接讓飛行器飛起來的，如果希望下載能夠讓自己飛機飛起來的程式碼建議下載QGroundControl地面站軟體，按照軟體的提示來選擇自己的機型，校正感測器，連結遙控器，除錯引數等操作。

3 PX4軟體架構分析

    PX4的軟體架構將會從4方面來展開陳述，分別是：軟體框架的劃分，程式碼檔案結構，飛行控制流程以及原始碼分析。其中原始碼分析將挑選具有代表性的返航控制模組，混控器以及uORB中介軟體來分析，這三部分分別隸屬於軟體框架層次中的應用程式框架，庫和作業系統。

3.1軟體框架的劃分

    PX4的程式碼開源在Github上，從https://github.com/PX4/Firmware上可以下載到原始碼。PX4的軟體建立在Nuttx作業系統上，它的大體框架如圖3所示：

圖3 軟體框架

    根據PX4軟體的運作，同時為了便於我們更好的理解，它可以劃分成四個層次：

1)    應用程式的API：這個介面提供給開發人員，比如使用ROS(實時作業系統)或DroneAPI(DroneAPI是一個用於控制無人機的Python庫，可以單獨的執行在機載電腦或者其他裝置上，通過串列埠或者無線的方式與飛控板通訊)，這一層旨在儘可能的精簡、扁平及隱藏其複雜性。

2)    應用程式框架: 這是為操作基礎飛行控制的預設程式集（節點）。比如旋翼的控制、直升機的控制、固定翼的控制等，還有位置估計、命令控制、導航控制等。

3)    庫: 這一層包含了所有的系統庫和基本交通控制的函式。比如數學庫、控制庫、MAV庫等等。

4)    作業系統: 最後一層提供作業系統支援，比如任務的排程，硬體驅動程式，網路，UAVCAN和故障安全系統等功能。

3.2 檔案結構

    分析完了整體的框架，對原始碼檔案結構的分析也同樣重要，原始碼的目錄結構如圖4所示。在firmware資料夾下有很多資料夾，我們按照圖片中的順序來講解每個資料夾中程式碼的功能。其中cmake是編譯工具；build_px4fmu-v2_default是在編譯後產生中間靜態檔案和最終生成的下載檔案；src包含飛行控制的主要程式碼；nuttx-config是nuttx的配置檔案；tools資料夾下包含一些工具，比如下載工具；Nuttx裡面則是Nuttx作業系統的原始碼；ROMFS裡面包含的是飛控板的啟動程式碼。

    下面介紹非常重要的src資料夾裡面包含的內容。systemcmds:主要放置了系統工具，能夠通過啟動檔案啟動或在nsh中去呼叫的工具。其中包括控制I2C,檢視修改引數，檢視軟體版本，校準電調、檢視系統效能、bootloader升級等工具；drivers裡面包含的主要是和硬體相關的驅動程式碼，比如stm32資料夾下面包括adc的基類、高精度定時器、伺服控制的驅動程式，device資料夾下面包含外設的基類定義，比如I2C和SPI等，boards目錄下定義了這個型號的板子的介面配置以及相應配置介面（LED、PWM、USB、定時器等的配置）；examples下面包含一些簡單的例項程式，如果有一些實驗性的程式碼可以放到這下面；include目錄和lib目錄包含其他程式碼需要用到的標頭檔案和庫；platforms下面定義了px4平臺的系統介面方便和nuttx作業系統分離，這樣方便移植到其他平臺；modules下面分了很多資料夾，這些資料夾各自是不同的模組，這就是最上層的功能模組包括commder、navigator、mc_att_control、mc_pos_control等。

圖4 檔案結構

3.3 飛行控制流程

    飛行控制的流程如圖5所示。圖中的左邊是飛控系統的控制功能實現，我們從上往下，從左往右依次分析。

1)    使用者通過地面站或者遙控器發出模式切換以及搖桿操作對飛行器進行控制，commander根據飛行器的當前狀態對使用者想要切換到的狀態main_state進行判斷：是否能夠切換到目標狀態，確定最終狀態。stickmapper見名知意，搖桿對映，參見px4io.c、sensors.cpp檔案。

2)    navigator決定飛行器最終會怎麼做飛航模式nav_state的轉換，以及對應模式下飛機的飛行控制。

3)    位置控制提取local postion(光流+IMU)或global postion(GPS)資料根據導航狀態進行串級PID控制。外環輸入位置差輸出速度，內環輸出推力。

4)    姿態控制採用傾轉分離的解耦合控制方式，先對齊Z軸再對齊偏航角。同樣採用串級PID的控制方式，外環輸入姿態角誤差，輸出姿態角速率，內環最後輸出所需的力矩。

5)    mixer混控器根據機型進行力矩分配。根據葉素理論，螺旋槳旋轉產生的s升力/扭矩與轉速的平方以及拉力系數/扭矩係數相關。構造力矩分配矩陣。

6)    motor_driver驅動電機，控制飛行器飛行。

左邊是位置估計演算法的實現，GPS確定global postion，optical flow確定local postion，二者有其一才能進入到POSCTL模式。姿態估計是一切的基礎，通過融合慣性感測器(和光流)各自優點解算出飛行器的當前姿態資訊。

圖5飛行控制流程

3.4 原始碼分析

    在這裡我們將結合原始碼分析3個模組的功能及實現，分別是RLT(Return to launch返航控制)模組，mixer(混控器模組)，uORB(Micro Object Request Broker,微物件請求代理器)訊息中介軟體。

3.4.1 RTL

    RTL(return to launch,返航模式)是應用程式框架Navigator中的一部分。它的功能顧名思義就是提供返航功能，返回並降落到“家”的位置。當飛航模式切換到返航模式時，飛行器會先爬升到返航設定的最低的高度，然後再家的上方懸停幾秒(可設定)，最後降落到“家”的位置，即飛行器起飛的位置或者說是飛行器解鎖時的位置。在RTL控制中，把返航飛行劃分成了7個部分，如圖6所示。

圖6 返航飛行的劃分

    飛航模式相對應的控制函式儲存在_navigation_mode_array的陣列裡面，如圖7所示，比如RTL模式的控制函式存放在第3個位置，在導航函式中對定期的檢查當前的模式，然後執行當前模式的程式碼，如圖8所示。當第一次執行_rtl函式時需要先呼叫on_activation()函式，非第一次執行_rtl函式則執行on_active函式，因為在第一次執行時需要對飛機進安全性檢查，具體可以看圖9的on_activation()函式，將依次進行如下判斷：

1)    如果當前已經著陸，不執行。

2)    如果當前正在執行降落命令，則直接切換到RTL_STATE_RETURN。

3)    如果高度小於最小返航高度，則爬升到設定高度，如果大於最小高度，則以當前高度返航。

4)    其他情況直接切換到RTL_STATE_RETURN。

5)    最後執行set_rtl_item函式，為不同狀態執行不同的控制。

圖7 _navigation_mode_array

圖8 導航模式的輪詢

圖9 on_activation()函式

3.4.2 mixer

Mixer(混控器)是根據機型進行力矩分配的庫，有了混控器使得PX4架構保證了核心控制器中不需要針對機身佈局做特別處理。混控指的是把輸入指令（例如：遙控器打右轉）分配到電機以及舵機的執行器（如電調或舵機PWM）指令。對於固定翼的副翼控制而言，每個副翼由一個舵機控制，那麼混控的意義就是控制其中一個副翼抬起而另一個副翼落下。同樣的，對多旋翼而言，俯仰操作需要改變所有電機的轉速。將混控邏輯從實際姿態控制器中分離出來可以大大提高複用性。Mixer簡要的控制流程是這樣的，一個特定的控制器（如姿態控制器）傳送特定的歸一化（-1..+1）的命令到給混合（mixing）,然後混合後輸出獨立的PWM到執行器（電調，舵機等）.在經過輸出驅動如（串列埠，UAVCAN，PWM）等將歸一化的值再轉回特性的值（如輸出1300的PWM等）。在混控器中每個機架都會有自己對於的混控檔案，比如旋翼對於的混控檔案是mixer_multirotor.cpp。

這裡拿四旋翼的X模式來舉例說明一下，機架的軸向及力矩分配矩陣如圖10所示：

 

圖10 機架軸向及力矩分配矩陣

    力矩分配矩陣中，每一行代表一個電機，並且和它們的編號順序相同，第1列到第三列分別代表的是Roll、Pitch和Yaw軸的輸入。比如當前需要加大Roll軸的角度，其他軸的的角度保持不變，那麼2,3號的電機轉速應該增加，1,4號電機的轉速應該較小，對比在力矩分配矩陣上你可以看到，第2,3行的第一列是正值，表示轉速和Roll的期望角度正相關，第1,4行是負值，代表和Roll的角度負相關。

    在混控器檔案中，首先會得到某個通道在對應軸向的力矩輸入，比如說來自飛機姿態控制器的輸入，或者是控制器的輸入等，範圍會控制在-1~1之間，得到輸入後，混控器會把輸入力矩的大小混合對映到每個電機上，同樣每個電機得到幅度為0~1範圍內的輸出(0代表停轉，1代表滿轉)，最後把這個輸出值計算出PWM的佔空比，然後通過電調來調節電機的轉速。混控器的控制流程分為下面幾步：

1)    讀取控制通道的輸入。

2)    先對Roll、Pitch的輸入通道進行計算，得到輸出值，因為姿態調節中首先考慮的是Roll和Pitch是否穩定，相對於Yaw則沒有那麼重要，然後根據計算得到的最大最小值判斷是否飽和。飽和的意思就是說每個電機的的輸出是否超過了0~1的範圍，如果超過和0~1的範圍但是最大最小值之間沒有超過1，則可以通過平移來解決，如果幅度超過了1，則需要通過縮放解決。

3)    確定Roll和Pitch軸可以滿足輸出要求後，這時加入Yaw，再次計算是否飽和，如果飽和適當調節油門來響應Yaw。

4)    最後得到每個電機的輸出，判斷改變幅度是否過大，做一個限幅。

原始碼分別如圖11,12,13,14所示：

圖11 得到通道0的輸入

圖12 對Roll和Pitch軸進行疊加計算

圖13 記錄下電機相差的的最大幅度

圖14 對電機的輸出進行評議或縮放操作

3.4.3 uORB分析

    uORB(Micro Object Request Broker,微物件請求代理器)是PX4/Pixhawk系統中非常重要且關鍵的一個模組，它肩負了整個系統的資料傳輸任務，所有的感測器資料、GPS、PPM訊號等都要從晶片獲取後通過uORB進行傳輸到各個模組進行計算處理。實際上uORB是一套跨「程式」 的IPC通訊模組。在Pixhawk中，所有的功能被獨立以程式模組為單位進行實現並工作。而程式間的資料互動就由為重要，必須要能夠符合實時、有序的特點。

    Pixhawk使用的是NuttX實時ARM系統，uORB實際上是多個程式開啟同一個裝置檔案，程式間通過此檔案節點進行資料互動和共享。程式通過命名的匯流排交換的訊息稱之為主題(topic)，在Pixhawk 中，一個主題僅包含一種訊息型別，通俗點就是資料型別。每個程式可以訂閱或者釋出主題，可以存在多個釋出者，或者一個程式可以訂閱多個主題，但是一條匯流排上始終只有一條訊息，如圖15所示。

圖15 uORB訂閱釋出示意圖

    在uORB中訂閱或者釋出主題的流程如圖16所示，首先使用orb_subscribe訂閱某個主題，訂閱後獲得一個控制程式碼，對控制程式碼初始化化，比如說初始化為檢測POLLIN事件，之後使用poll函式來監視檔案描述符，如果對應的實際發生，使用orb_copy來獲得對應的訊息。

圖16 uORB訂閱/釋出資料流程

接下來對一些常用的uORB函式進行介紹。

1)    int orb_subscribe(const structorb_metadata *meta)

功能：訂閱主題（topic）;

說明：即使訂閱的主題沒有被公告，但是也能訂閱成功；但是在這種情況下，卻得不到資料，直到主題被公告；

引數：

    meta:uORB元物件，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值；

返回值：

    錯誤則返回ERROR;成功則返回一個可以讀取資料、更新話題的控制程式碼；如果待訂閱的主題沒有定義或宣告則會返回-1，然後會將errno賦值為ENOENT;

eg:

int fd =orb_subscribe(ORB_ID(topicName));

2)    int poll(struct pollfd fds[],nfds_t nfds, int timeout)

功能：監控檔案描述符（多個）；

說明：timemout=0,poll()函式立即返回而不阻塞；timeout=INFTIM(-1),poll()會一直阻塞下去，直到檢測到return > 0；

引數：

    fds:struct pollfd結構型別的陣列；

    nfds:用於標記陣列fds中的結構體元素的總數量；

    timeout:是poll函式呼叫阻塞的時間，單位：毫秒；

返回值：

    >0：陣列fds中準備好讀、寫或出錯狀態的那些socket描述符的總數量；

    ==0:poll()函式會阻塞timeout所指定的毫秒時間長度之後返回;

-1:poll函式呼叫失敗；同時會自動設定全域性變數errno；

3) intorb_copy(const struct orb_metadata *meta, int handle, void *buffer)

功能：從訂閱的主題中獲取資料並將資料儲存到buffer中；

引數：

    meta:uORB元物件，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;

    handle:訂閱主題返回的控制程式碼；

    buffer:從主題中獲取的資料；

返回值：

    返回OK表示獲取資料成功，錯誤返回ERROR;否則則有根據的去設定errno;

eg:

    struct sensor_combined_s raw;

orb_copy(ORB_ID(sensor_combined),sensor_sub_fd, &raw);

4)orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)

功能：公告發布者的主題；

說明：在釋出主題之前是必須的；否則訂閱者雖然能訂閱，但是得不到資料；

引數：

    meta:uORB元物件，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;

    data:指向一個已被初始化，釋出者要釋出的資料儲存變數的指標；

返回值：錯誤則返回ERROR;成功則返回一個可以釋出主題的控制程式碼；如果待發布的主題沒有定義或宣告則會返回-1，然後會將errno賦值為ENOENT;

eg:

    struct vehicle_attitude_s att;

    memset(&att, 0, sizeof(att));

intatt_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);

5)    int orb_publish(const structorb_metadata *meta, orb_advert_t handle, const void *data)

功能：釋出新資料到主題；

引數：

    meta:uORB元物件，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;

    handle:orb_advertise函式返回的控制程式碼；

    data:指向待發布資料的指標；

返回值：OK表示成功；錯誤返回ERROR；否則則有根據的去設定errno;

eg:

    orb_publish(ORB_ID(vehicle_attitude),att_pub_fd, &att);

4 總結

Pixhawk硬體的功能已經能夠滿足絕大部分飛行器的硬體需求了，同時由於硬體開源的特點，相信以後的硬體功能也會越來越強大。PX4軟體程式碼建立在Nuttx作業系統上，穩定性可以得到很大的保障，其使用的uORB訊息中介軟體技術也大大的簡化了開發，同時PX4模組化的程式設計思想也為開發人員的閱讀和二次開發提供了很大的便利。