pixhawk学习

 原文转载至https://blog.csdn.net/u013181595/article/details/80976610

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向安装脚本传入其它地址。安装脚本会自动配置需要的环境变量。最后，运行以下命令更新环境变量：

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模块化的编程思想也为开发人员的阅读和二次开发提供了很大的便利。
---------------------  
作者：dayL_W  
来源：CSDN  
原文：https://blog.csdn.net/u013181595/article/details/80976610  
版权声明：本文为博主原创文章，转载请附上博文链接！