前段時(shí)間導(dǎo)師叫我做撲翼無(wú)人機(jī)，工程上需要實(shí)現(xiàn)的，能夠通過(guò)程控飛起來(lái)，感覺(jué)難度挺大。先從研究PX4開(kāi)始，打算一步步理解透整個(gè)PX4的框架，機(jī)型的適配、旋翼、固定翼的姿態(tài)控制，新機(jī)型的添加等等。不知道能不能做成，這里先立個(gè)flag吧。

    這篇文檔是我課程作業(yè)的一個(gè)報(bào)告，包含4個(gè)方面，硬件架構(gòu)分析，Linux開(kāi)發(fā)環(huán)境搭建，軟件架構(gòu)分析，源代碼分析等。由于源代碼比較龐雜，這里簡(jiǎn)要分析下飛機(jī)的返航控制(RTL)，旋翼的混控器，以及uORB的使用三個(gè)方面。

Pixhawk是一個(gè)獨(dú)立的項(xiàng)目，旨在以低成本和高可用性為學(xué)術(shù)界，業(yè)余愛(ài)好界和工業(yè)界提供高端行業(yè)標(biāo)準(zhǔn)自動(dòng)駕駛儀硬件。由于其強(qiáng)大且豐富的硬件而廣受無(wú)人機(jī)愛(ài)好者的好評(píng)。Pixhawk支持APM和PX4的固件，這里我們對(duì)原生的PX4固件做軟件架構(gòu)分析。對(duì)于Pixhawk的系統(tǒng)架構(gòu)分析我將從四方面來(lái)展開(kāi)，分別是：硬件架構(gòu)介紹、PX4開(kāi)發(fā)環(huán)境搭建、PX4軟件架構(gòu)分析及總結(jié)。這里我們主要對(duì)軟件架構(gòu)展開(kāi)分析，根據(jù)架構(gòu)的層次性對(duì)PX4中的導(dǎo)航模塊、Mixer模塊和uORB模塊做分析和介紹。

關(guān)鍵詞：Pixhawk、PX4、硬件架構(gòu)、軟件架構(gòu)、uORB

1硬件架構(gòu)分析

    Pixhawk是一款基于ARM芯片的32位開(kāi)源飛控，由ETH的computer vision and geometry group的博士生Lorenz Meier開(kāi)發(fā)。最初采用的是分體式的設(shè)計(jì)即px4（由px4fmu和px4io兩個(gè)組件組成），后合并成一個(gè)整體形成現(xiàn)在的pixhawk。其硬件和軟件都開(kāi)源，因此衍生出很多不同的軟硬件版本，最初的分銷商是美國(guó)的3D Robotics。PIXHAWK是一款高性能自動(dòng)駕駛儀，適用于固定翼，多旋翼，直升機(jī)，汽車，船只以及任何可移動(dòng)的其他機(jī)器人平臺(tái)。它針對(duì)高端研究，業(yè)余和行業(yè)需求，并結(jié)合了PX4FMU PX4IO的功能。Pixhawk的外形如圖1所示：

圖1 Pixhawk外形及接口

    Pixhawk采用帶有FPU的32位STM32F427芯片，它的采用Cortex M4內(nèi)核，主主頻為168M，具有252MIPS的運(yùn)算能力、256KB的RAM以及2MB的閃存，同時(shí)具有一片型號(hào)為STM32F103的故障協(xié)處理器芯片。在傳感器的選用上面，它選用ST公司的MicroL3GD20H 16位陀螺儀和MicroLSM303D 14位加速度計(jì)/磁力計(jì)，選用Invensense公司MPU 6000 3軸加速度計(jì)/陀螺儀，采用MEAS公司的MS5611氣壓計(jì)。同時(shí)擁有非常豐富的硬件接口，它擁有5個(gè)UART（串行端口），其中一個(gè)具有高功率，2個(gè)帶HW流量控制，2個(gè)CAN（一個(gè)帶內(nèi)部3.3V收發(fā)器，一個(gè)帶擴(kuò)展連接器）接口，Spektrum DSM / DSM2 / DSM-X衛(wèi)星兼容輸入接口，F(xiàn)utabaS.BUS兼容輸入和輸出接口，PPM和信號(hào)輸入接口，RSSI（PWM或電壓）輸入接口，同時(shí)還有I2C、SPI、ADC、內(nèi)部和外部microUSB接口等等。其豐富的接口可以滿足任何可移動(dòng)的機(jī)器人平臺(tái)的需要。在電力系統(tǒng)與保護(hù)方面，Pixhawk具有自動(dòng)故障轉(zhuǎn)移功能的理想二極管，能夠適應(yīng)大功率的伺服電機(jī)（最大10V）和大電流（10A ），所有外圍輸出均過(guò)流保護(hù)，所有輸入均受ESD保護(hù)。Pixhawk提供三個(gè)電源，它可以在電源上實(shí)現(xiàn)三重冗余。三個(gè)電源分別是：電源模塊輸入，伺服電機(jī)輸入，USB輸入。它優(yōu)先使用電源模塊的輸入，電壓范圍為4.8V到5.4V，其次是伺服電機(jī)的電壓輸入，電壓范圍也為4.8V到5.4V，最后選擇USB的電源輸入，電壓范圍也是一樣的。Pixhawk的原理圖可以從https://raw./PX4/Hardware/master/FMUv2/PX4FMUv2.4.5.pdf處下載。

Pixhawk的硬件架構(gòu)整體框圖如圖2所示：

圖2 硬件框架

    由圖中的內(nèi)容可以看出硬件的大致框架，STM32F427芯片作為主控器承擔(dān)著全部傳感器的數(shù)據(jù)讀寫，姿態(tài)的結(jié)算及控制，以及其他信息的處理和控制。電源系統(tǒng)不僅為電機(jī)電調(diào)提供電源，同時(shí)還承擔(dān)著主控制器器和所有模塊的電源供應(yīng)。對(duì)于其它外圍系統(tǒng)以及和主控器的通信方式分別如下所示：動(dòng)力系統(tǒng)由主控器的四路PWM控制，調(diào)試系統(tǒng)通過(guò)microUSB接口和主控器交互，文件系統(tǒng)通過(guò)SDIO協(xié)議和主控制器交互，數(shù)傳系統(tǒng)通過(guò)433M模塊和主控器交互，遙控系統(tǒng)通過(guò)SPI協(xié)議和主控制器交互，導(dǎo)航系統(tǒng)通過(guò)RTMC協(xié)議和主控器交互，定高及姿態(tài)控制系統(tǒng)都是通過(guò)I2C協(xié)議和主控器交互。這里比較故障協(xié)處理器比較有趣，當(dāng)飛機(jī)的主控制器跑飛或者發(fā)生其他錯(cuò)誤導(dǎo)致無(wú)法對(duì)電機(jī)進(jìn)行控制時(shí)，可以由協(xié)控制器感知到，進(jìn)而獲得電機(jī)的控制權(quán)，將電機(jī)控制在安全的模式下，直到檢測(cè)到主控器對(duì)電機(jī)的控制恢復(fù)正常。協(xié)控制器不定時(shí)的讀取主控器的DMA計(jì)數(shù)寄存器，當(dāng)這次計(jì)數(shù)寄存器的值和上去讀取到的值一致時(shí)說(shuō)明主控器沒(méi)有對(duì)電機(jī)的PWM進(jìn)行更新，主控制器失去了對(duì)電機(jī)的控制，這時(shí)協(xié)控制器得知主控制器異常，開(kāi)始接管對(duì)電機(jī)的控制，直到DMA計(jì)數(shù)器更新，主控制器恢復(fù)對(duì)電機(jī)的控制。

2 PX4開(kāi)發(fā)環(huán)境搭建

    PX4代碼可以在 Mac OS,Linux 或者 Windows上進(jìn)行開(kāi)發(fā)，建議在Mac OS和Linux上進(jìn)行開(kāi)發(fā)，因?yàn)閳D像處理和高級(jí)導(dǎo)航在windows上不容易開(kāi)發(fā)。如果不確定，新的開(kāi)發(fā)者應(yīng)默認(rèn)用Linux和當(dāng)前Ubuntu長(zhǎng)期支持版本（Ubuntu LTS edition）。如果對(duì)Docker熟悉，你可以使用其中一個(gè)容器來(lái)安裝開(kāi)發(fā)環(huán)境。

    我們使用Debian /Ubuntu LTS 作為L(zhǎng)inux的標(biāo)準(zhǔn)支持版本，在這里我的Linux版本為Ubuntu 16.04。

2.1 權(quán)限設(shè)置

    為了便于開(kāi)發(fā)，我們需要把用戶添加到用戶組”dialout”中，執(zhí)行：

sudo usermod -a -G dialout $USER

    然后注銷后，重新登錄，因?yàn)橹匦碌卿浐笏龅母淖儾艜?huì)有效。注意：永遠(yuǎn)不要使用sudo來(lái)修復(fù)權(quán)限問(wèn)題，否則會(huì)帶來(lái)更多的權(quán)限問(wèn)題，需要重裝系統(tǒng)來(lái)解決。

2.2 安裝PX4的依賴包

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

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配備了一系列代理管理，這會(huì)嚴(yán)重干擾任何機(jī)器人相關(guān)的串口（或usb串口），卸載掉它也不會(huì)有什么影響，我們這里把它卸載：

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工具鏈之前，請(qǐng)確保刪除殘余：

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

如果需要在樹(shù)莓派上開(kāi)發(fā)則需要安裝樹(shù)莓派上對(duì)于的工具鏈。樹(shù)莓派開(kāi)發(fā)者應(yīng)該從下面地址下載樹(shù)莓派Linux工具鏈。安裝腳本會(huì)自動(dòng)安裝交叉編譯工具鏈：

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

cd rpi_toolchain

./install_cross.sh

在工具鏈安裝過(guò)程中需要輸入密碼。如果不想把工具鏈安裝在默認(rèn)位置/opt/rpi_toolchain，可以執(zhí)行./install_cross.sh<PATH>向安裝腳本傳入其它地址。安裝腳本會(huì)自動(dòng)配置需要的環(huán)境變量。最后，運(yùn)行以下命令更新環(huán)境變量：

source ~/.profile

2.4 代碼編譯

    開(kāi)發(fā)環(huán)境搭建好之后，就可以開(kāi)始下載代碼并編譯了，PX4可以在控制臺(tái)或者圖形界面/IDE開(kāi)發(fā)。在這里我們對(duì)控制臺(tái)的開(kāi)發(fā)進(jìn)行簡(jiǎn)要介紹。

    先建立工作目錄，然后從git上下載代碼：

mkdir -p ~/src

cd ~/src

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

cd Firmware

git submodule update --init --recursive

cd ..

    現(xiàn)在可以通過(guò)編譯源代碼來(lái)構(gòu)建二進(jìn)制文件。在直接使用硬件前，推薦先進(jìn)行仿真。然后就可以對(duì)代碼進(jìn)行編譯并下載了：

cd Firmware

make px4fmu-v2_default

    注意到“make”是一個(gè)字符命令編譯工具，“px4fmu-v2”是硬件/ardupilot版本，“default”是默認(rèn)配置，所有的PX4編譯目標(biāo)遵循這個(gè)規(guī)則。通過(guò)在命令后面添加'upload’，編譯的二進(jìn)制程序就會(huì)通過(guò)USB上傳到飛控硬件。下載成功后的情況如下所示：

    當(dāng)然這樣編譯后的代碼是無(wú)法直接讓飛行器飛起來(lái)的，如果希望下載能夠讓自己飛機(jī)飛起來(lái)的代碼建議下載QGroundControl地面站軟件，按照軟件的提示來(lái)選擇自己的機(jī)型，校正傳感器，鏈接遙控器，調(diào)試參數(shù)等操作。

3 PX4軟件架構(gòu)分析

    PX4的軟件架構(gòu)將會(huì)從4方面來(lái)展開(kāi)陳述，分別是：軟件框架的劃分，代碼文件結(jié)構(gòu)，飛行控制流程以及源代碼分析。其中源代碼分析將挑選具有代表性的返航控制模塊，混控器以及uORB中間件來(lái)分析，這三部分分別隸屬于軟件框架層次中的應(yīng)用程序框架，庫(kù)和操作系統(tǒng)。

3.1軟件框架的劃分

    PX4的代碼開(kāi)源在Github上，從https://github.com/PX4/Firmware上可以下載到源代碼。PX4的軟件建立在Nuttx操作系統(tǒng)上，它的大體框架如圖3所示：

圖3 軟件框架

    根據(jù)PX4軟件的運(yùn)作，同時(shí)為了便于我們更好的理解，它可以劃分成四個(gè)層次：

1)    應(yīng)用程序的API：這個(gè)接口提供給開(kāi)發(fā)人員，比如使用ROS(實(shí)時(shí)操作系統(tǒng))或DroneAPI(DroneAPI是一個(gè)用于控制無(wú)人機(jī)的Python庫(kù)，可以單獨(dú)的運(yùn)行在機(jī)載電腦或者其他設(shè)備上，通過(guò)串口或者無(wú)線的方式與飛控板通信)，這一層旨在盡可能的精簡(jiǎn)、扁平及隱藏其復(fù)雜性。

2)    應(yīng)用程序框架: 這是為操作基礎(chǔ)飛行控制的默認(rèn)程序集（節(jié)點(diǎn)）。比如旋翼的控制、直升機(jī)的控制、固定翼的控制等，還有位置估計(jì)、命令控制、導(dǎo)航控制等。

3)    庫(kù): 這一層包含了所有的系統(tǒng)庫(kù)和基本交通控制的函數(shù)。比如數(shù)學(xué)庫(kù)、控制庫(kù)、MAV庫(kù)等等。

4)    操作系統(tǒng): 最后一層提供操作系統(tǒng)支持，比如任務(wù)的調(diào)度，硬件驅(qū)動(dòng)程序，網(wǎng)絡(luò)，UAVCAN和故障安全系統(tǒng)等功能。

3.2 文件結(jié)構(gòu)

    分析完了整體的框架，對(duì)源代碼文件結(jié)構(gòu)的分析也同樣重要，源代碼的目錄結(jié)構(gòu)如圖4所示。在firmware文件夾下有很多文件夾，我們按照?qǐng)D片中的順序來(lái)講解每個(gè)文件夾中代碼的功能。其中cmake是編譯工具；build_px4fmu-v2_default是在編譯后產(chǎn)生中間靜態(tài)文件和最終生成的下載文件；src包含飛行控制的主要代碼；nuttx-config是nuttx的配置文件；tools文件夾下包含一些工具，比如下載工具；Nuttx里面則是Nuttx操作系統(tǒng)的源代碼；ROMFS里面包含的是飛控板的啟動(dòng)代碼。

    下面介紹非常重要的src文件夾里面包含的內(nèi)容。systemcmds:主要放置了系統(tǒng)工具，能夠通過(guò)啟動(dòng)文件啟動(dòng)或在nsh中去調(diào)用的工具。其中包括控制I2C,查看修改參數(shù)，查看軟件版本，校準(zhǔn)電調(diào)、查看系統(tǒng)性能、bootloader升級(jí)等工具；drivers里面包含的主要是和硬件相關(guān)的驅(qū)動(dòng)代碼，比如stm32文件夾下面包括adc的基類、高精度定時(shí)器、伺服控制的驅(qū)動(dòng)程序，device文件夾下面包含外設(shè)的基類定義，比如I2C和SPI等，boards目錄下定義了這個(gè)型號(hào)的板子的接口配置以及相應(yīng)配置接口（LED、PWM、USB、定時(shí)器等的配置）；examples下面包含一些簡(jiǎn)單的實(shí)例程序，如果有一些實(shí)驗(yàn)性的代碼可以放到這下面；include目錄和lib目錄包含其他代碼需要用到的頭文件和庫(kù)；platforms下面定義了px4平臺(tái)的系統(tǒng)接口方便和nuttx操作系統(tǒng)分離，這樣方便移植到其他平臺(tái)；modules下面分了很多文件夾，這些文件夾各自是不同的模塊，這就是最上層的功能模塊包括commder、navigator、mc_att_control、mc_pos_control等。

圖4 文件結(jié)構(gòu)

3.3 飛行控制流程

    飛行控制的流程如圖5所示。圖中的左邊是飛控系統(tǒng)的控制功能實(shí)現(xiàn)，我們從上往下，從左往右依次分析。

1)    用戶通過(guò)地面站或者遙控器發(fā)出模式切換以及搖桿操作對(duì)飛行器進(jìn)行控制，commander根據(jù)飛行器的當(dāng)前狀態(tài)對(duì)用戶想要切換到的狀態(tài)main_state進(jìn)行判斷：是否能夠切換到目標(biāo)狀態(tài)，確定最終狀態(tài)。stickmapper見(jiàn)名知意，搖桿映射，參見(jiàn)px4io.c、sensors.cpp文件。

2)    navigator決定飛行器最終會(huì)怎么做飛行模式nav_state的轉(zhuǎn)換，以及對(duì)應(yīng)模式下飛機(jī)的飛行控制。

3)    位置控制提取local postion(光流 IMU)或global postion(GPS)數(shù)據(jù)根據(jù)導(dǎo)航狀態(tài)進(jìn)行串級(jí)PID控制。外環(huán)輸入位置差輸出速度，內(nèi)環(huán)輸出推力。

4)    姿態(tài)控制采用傾轉(zhuǎn)分離的解耦合控制方式，先對(duì)齊Z軸再對(duì)齊偏航角。同樣采用串級(jí)PID的控制方式，外環(huán)輸入姿態(tài)角誤差，輸出姿態(tài)角速率，內(nèi)環(huán)最后輸出所需的力矩。

5)    mixer混控器根據(jù)機(jī)型進(jìn)行力矩分配。根據(jù)葉素理論，螺旋槳旋轉(zhuǎn)產(chǎn)生的s升力/扭矩與轉(zhuǎn)速的平方以及拉力系數(shù)/扭矩系數(shù)相關(guān)。構(gòu)造力矩分配矩陣。

6)    motor_driver驅(qū)動(dòng)電機(jī)，控制飛行器飛行。

左邊是位置估計(jì)算法的實(shí)現(xiàn)，GPS確定global postion，optical flow確定local postion，二者有其一才能進(jìn)入到POSCTL模式。姿態(tài)估計(jì)是一切的基礎(chǔ)，通過(guò)融合慣性傳感器(和光流)各自優(yōu)點(diǎn)解算出飛行器的當(dāng)前姿態(tài)信息。

圖5飛行控制流程

3.4 源代碼分析

    在這里我們將結(jié)合源代碼分析3個(gè)模塊的功能及實(shí)現(xiàn)，分別是RLT(Return to launch返航控制)模塊，mixer(混控器模塊)，uORB(Micro Object Request Broker,微對(duì)象請(qǐng)求代理器)消息中間件。

3.4.1 RTL

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

圖6 返航飛行的劃分

    飛行模式相對(duì)應(yīng)的控制函數(shù)存儲(chǔ)在_navigation_mode_array的數(shù)組里面，如圖7所示，比如RTL模式的控制函數(shù)存放在第3個(gè)位置，在導(dǎo)航函數(shù)中對(duì)定期的檢查當(dāng)前的模式，然后運(yùn)行當(dāng)前模式的代碼，如圖8所示。當(dāng)?shù)谝淮芜\(yùn)行_rtl函數(shù)時(shí)需要先調(diào)用on_activation()函數(shù)，非第一次運(yùn)行_rtl函數(shù)則運(yùn)行on_active函數(shù)，因?yàn)樵诘谝淮芜\(yùn)行時(shí)需要對(duì)飛機(jī)進(jìn)安全性檢查，具體可以看圖9的on_activation()函數(shù)，將依次進(jìn)行如下判斷：

1)    如果當(dāng)前已經(jīng)著陸，不執(zhí)行。

2)    如果當(dāng)前正在執(zhí)行降落命令，則直接切換到RTL_STATE_RETURN。

3)    如果高度小于最小返航高度，則爬升到設(shè)定高度，如果大于最小高度，則以當(dāng)前高度返航。

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

5)    最后執(zhí)行set_rtl_item函數(shù)，為不同狀態(tài)執(zhí)行不同的控制。

圖7 _navigation_mode_array

圖8 導(dǎo)航模式的輪詢

圖9 on_activation()函數(shù)

3.4.2 mixer

Mixer(混控器)是根據(jù)機(jī)型進(jìn)行力矩分配的庫(kù)，有了混控器使得PX4架構(gòu)保證了核心控制器中不需要針對(duì)機(jī)身布局做特別處理。混控指的是把輸入指令（例如：遙控器打右轉(zhuǎn)）分配到電機(jī)以及舵機(jī)的執(zhí)行器（如電調(diào)或舵機(jī)PWM）指令。對(duì)于固定翼的副翼控制而言，每個(gè)副翼由一個(gè)舵機(jī)控制，那么混控的意義就是控制其中一個(gè)副翼抬起而另一個(gè)副翼落下。同樣的，對(duì)多旋翼而言，俯仰操作需要改變所有電機(jī)的轉(zhuǎn)速。將混控邏輯從實(shí)際姿態(tài)控制器中分離出來(lái)可以大大提高復(fù)用性。Mixer簡(jiǎn)要的控制流程是這樣的，一個(gè)特定的控制器（如姿態(tài)控制器）發(fā)送特定的歸一化（-1.. 1）的命令到給混合（mixing）,然后混合后輸出獨(dú)立的PWM到執(zhí)行器（電調(diào)，舵機(jī)等）.在經(jīng)過(guò)輸出驅(qū)動(dòng)如（串口，UAVCAN，PWM）等將歸一化的值再轉(zhuǎn)回特性的值（如輸出1300的PWM等）。在混控器中每個(gè)機(jī)架都會(huì)有自己對(duì)于的混控文件，比如旋翼對(duì)于的混控文件是mixer_multirotor.cpp。

這里拿四旋翼的X模式來(lái)舉例說(shuō)明一下，機(jī)架的軸向及力矩分配矩陣如圖10所示：

圖10 機(jī)架軸向及力矩分配矩陣

    力矩分配矩陣中，每一行代表一個(gè)電機(jī)，并且和它們的編號(hào)順序相同，第1列到第三列分別代表的是Roll、Pitch和Yaw軸的輸入。比如當(dāng)前需要加大Roll軸的角度，其他軸的的角度保持不變，那么2,3號(hào)的電機(jī)轉(zhuǎn)速應(yīng)該增加，1,4號(hào)電機(jī)的轉(zhuǎn)速應(yīng)該較小，對(duì)比在力矩分配矩陣上你可以看到，第2,3行的第一列是正值，表示轉(zhuǎn)速和Roll的期望角度正相關(guān)，第1,4行是負(fù)值，代表和Roll的角度負(fù)相關(guān)。

    在混控器文件中，首先會(huì)得到某個(gè)通道在對(duì)應(yīng)軸向的力矩輸入，比如說(shuō)來(lái)自飛機(jī)姿態(tài)控制器的輸入，或者是控制器的輸入等，范圍會(huì)控制在-1~1之間，得到輸入后，混控器會(huì)把輸入力矩的大小混合映射到每個(gè)電機(jī)上，同樣每個(gè)電機(jī)得到幅度為0~1范圍內(nèi)的輸出(0代表停轉(zhuǎn)，1代表滿轉(zhuǎn))，最后把這個(gè)輸出值計(jì)算出PWM的占空比，然后通過(guò)電調(diào)來(lái)調(diào)節(jié)電機(jī)的轉(zhuǎn)速?；炜仄鞯目刂屏鞒谭譃橄旅鎺撞剑?/p>

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

2)    先對(duì)Roll、Pitch的輸入通道進(jìn)行計(jì)算，得到輸出值，因?yàn)樽藨B(tài)調(diào)節(jié)中首先考慮的是Roll和Pitch是否穩(wěn)定，相對(duì)于Yaw則沒(méi)有那么重要，然后根據(jù)計(jì)算得到的最大最小值判斷是否飽和。飽和的意思就是說(shuō)每個(gè)電機(jī)的的輸出是否超過(guò)了0~1的范圍，如果超過(guò)和0~1的范圍但是最大最小值之間沒(méi)有超過(guò)1，則可以通過(guò)平移來(lái)解決，如果幅度超過(guò)了1，則需要通過(guò)縮放解決。

3)    確定Roll和Pitch軸可以滿足輸出要求后，這時(shí)加入Yaw，再次計(jì)算是否飽和，如果飽和適當(dāng)調(diào)節(jié)油門來(lái)響應(yīng)Yaw。

4)    最后得到每個(gè)電機(jī)的輸出，判斷改變幅度是否過(guò)大，做一個(gè)限幅。

源代碼分別如圖11,12,13,14所示：

圖11 得到通道0的輸入

圖12 對(duì)Roll和Pitch軸進(jìn)行疊加計(jì)算

圖13 記錄下電機(jī)相差的的最大幅度

圖14 對(duì)電機(jī)的輸出進(jìn)行評(píng)議或縮放操作

3.4.3 uORB分析

    uORB(Micro Object Request Broker,微對(duì)象請(qǐng)求代理器)是PX4/Pixhawk系統(tǒng)中非常重要且關(guān)鍵的一個(gè)模塊，它肩負(fù)了整個(gè)系統(tǒng)的數(shù)據(jù)傳輸任務(wù)，所有的傳感器數(shù)據(jù)、GPS、PPM信號(hào)等都要從芯片獲取后通過(guò)uORB進(jìn)行傳輸?shù)礁鱾€(gè)模塊進(jìn)行計(jì)算處理。實(shí)際上uORB是一套跨「進(jìn)程」 的IPC通訊模塊。在Pixhawk中，所有的功能被獨(dú)立以進(jìn)程模塊為單位進(jìn)行實(shí)現(xiàn)并工作。而進(jìn)程間的數(shù)據(jù)交互就由為重要，必須要能夠符合實(shí)時(shí)、有序的特點(diǎn)。

    Pixhawk使用的是NuttX實(shí)時(shí)ARM系統(tǒng)，uORB實(shí)際上是多個(gè)進(jìn)程打開(kāi)同一個(gè)設(shè)備文件，進(jìn)程間通過(guò)此文件節(jié)點(diǎn)進(jìn)行數(shù)據(jù)交互和共享。進(jìn)程通過(guò)命名的總線交換的消息稱之為主題(topic)，在Pixhawk 中，一個(gè)主題僅包含一種消息類型，通俗點(diǎn)就是數(shù)據(jù)類型。每個(gè)進(jìn)程可以訂閱或者發(fā)布主題，可以存在多個(gè)發(fā)布者，或者一個(gè)進(jìn)程可以訂閱多個(gè)主題，但是一條總線上始終只有一條消息，如圖15所示。

圖15 uORB訂閱發(fā)布示意圖

    在uORB中訂閱或者發(fā)布主題的流程如圖16所示，首先使用orb_subscribe訂閱某個(gè)主題，訂閱后獲得一個(gè)句柄，對(duì)句柄初始化化，比如說(shuō)初始化為檢測(cè)POLLIN事件，之后使用poll函數(shù)來(lái)監(jiān)視文件描述符，如果對(duì)應(yīng)的實(shí)際發(fā)生，使用orb_copy來(lái)獲得對(duì)應(yīng)的消息。

圖16 uORB訂閱/發(fā)布數(shù)據(jù)流程

接下來(lái)對(duì)一些常用的uORB函數(shù)進(jìn)行介紹。

1)    int orb_subscribe(const structorb_metadata *meta)

功能：訂閱主題（topic）;
說(shuō)明：即使訂閱的主題沒(méi)有被公告，但是也能訂閱成功；但是在這種情況下，卻得不到數(shù)據(jù)，直到主題被公告；
參數(shù)：
    meta:uORB元對(duì)象，可以認(rèn)為是主題id，一般是通過(guò)ORB_ID(主題名)來(lái)賦值；
返回值：
    錯(cuò)誤則返回ERROR;成功則返回一個(gè)可以讀取數(shù)據(jù)、更新話題的句柄；如果待訂閱的主題沒(méi)有定義或聲明則會(huì)返回-1，然后會(huì)將errno賦值為ENOENT;
eg:
int fd =orb_subscribe(ORB_ID(topicName));

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

功能：監(jiān)控文件描述符（多個(gè)）；
說(shuō)明：timemout=0,poll()函數(shù)立即返回而不阻塞；timeout=INFTIM(-1),poll()會(huì)一直阻塞下去，直到檢測(cè)到return > 0；
參數(shù)：
    fds:struct pollfd結(jié)構(gòu)類型的數(shù)組；
    nfds:用于標(biāo)記數(shù)組fds中的結(jié)構(gòu)體元素的總數(shù)量；
    timeout:是poll函數(shù)調(diào)用阻塞的時(shí)間，單位：毫秒；
返回值：
    >0：數(shù)組fds中準(zhǔn)備好讀、寫或出錯(cuò)狀態(tài)的那些socket描述符的總數(shù)量；
    ==0:poll()函數(shù)會(huì)阻塞timeout所指定的毫秒時(shí)間長(zhǎng)度之后返回;
-1:poll函數(shù)調(diào)用失敗；同時(shí)會(huì)自動(dòng)設(shè)置全局變量errno；

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

功能：從訂閱的主題中獲取數(shù)據(jù)并將數(shù)據(jù)保存到buffer中；
參數(shù)：
    meta:uORB元對(duì)象，可以認(rèn)為是主題id，一般是通過(guò)ORB_ID(主題名)來(lái)賦值;
    handle:訂閱主題返回的句柄；
    buffer:從主題中獲取的數(shù)據(jù)；
返回值：
    返回OK表示獲取數(shù)據(jù)成功，錯(cuò)誤返回ERROR;否則則有根據(jù)的去設(shè)置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)

功能：公告發(fā)布者的主題；
說(shuō)明：在發(fā)布主題之前是必須的；否則訂閱者雖然能訂閱，但是得不到數(shù)據(jù)；
參數(shù)：
    meta:uORB元對(duì)象，可以認(rèn)為是主題id，一般是通過(guò)ORB_ID(主題名)來(lái)賦值;
    data:指向一個(gè)已被初始化，發(fā)布者要發(fā)布的數(shù)據(jù)存儲(chǔ)變量的指針；
返回值：錯(cuò)誤則返回ERROR;成功則返回一個(gè)可以發(fā)布主題的句柄；如果待發(fā)布的主題沒(méi)有定義或聲明則會(huì)返回-1，然后會(huì)將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)

功能：發(fā)布新數(shù)據(jù)到主題；
參數(shù)：
    meta:uORB元對(duì)象，可以認(rèn)為是主題id，一般是通過(guò)ORB_ID(主題名)來(lái)賦值;
    handle:orb_advertise函數(shù)返回的句柄；
    data:指向待發(fā)布數(shù)據(jù)的指針；
返回值：OK表示成功；錯(cuò)誤返回ERROR；否則則有根據(jù)的去設(shè)置errno;
eg:
    orb_publish(ORB_ID(vehicle_attitude),att_pub_fd, &att);

4 總結(jié)

Pixhawk硬件的功能已經(jīng)能夠滿足絕大部分飛行器的硬件需求了，同時(shí)由于硬件開(kāi)源的特點(diǎn)，相信以后的硬件功能也會(huì)越來(lái)越強(qiáng)大。PX4軟件代碼建立在Nuttx操作系統(tǒng)上，穩(wěn)定性可以得到很大的保障，其使用的uORB消息中間件技術(shù)也大大的簡(jiǎn)化了開(kāi)發(fā)，同時(shí)PX4模塊化的編程思想也為開(kāi)發(fā)人員的閱讀和二次開(kāi)發(fā)提供了很大的便利。