无人机飞控开发-pre

图列 表列 引 项目研发背 国内外研究概 技术指 技术指标完成情 项目的总体研究思 系统硬件设 电源管理设 电源管理模 FMU和IO电源供 电 伺服装置供 备用电供 电 MCU 扩展接口电路设 电机驱 继电器接 USB接口电路设 SD GPS和安全开 蜂鸣 传感器模 主板传感 IMU载板传感 传感器冗余设 SPI通信协 MPU9250数据分 系统设 应用 中间 飞行控制 处理器的设 主处理器的设 协处理设 飞控算法设 姿态解算算法设 姿态控制算法设 位置估计算法设 位置控制算法设 输出混 健康评估与失效保 决策层的目的和意 安全问 通信故 传感器失 动力系统异 参数配置错 健康评 飞行前健康评 飞行中健康评 失控保 6.4.1.失联和失效保护 传感器故障失效保 动力系统异常失效保 安全决策实 飞控板的使用说 飞控板的接 飞控性 图列表图 Draganflyer四轴飞行 图 斯坦福大学改进的四轴飞行 图 宾夕法尼亚大学带臂抓取无人 图 Quattrocopter外形 图 AR.drone飞行器 图 大疆精灵 图 KK四轴控制系统 图 MK四轴飞行器控制系 图 MWC四轴控制系 图 ArduPilot历 图 系统整体框 图 飞控硬件总体方 图 飞控系统架 图 整体电路设 图 电源电路设 图 图 BQ24313/5电源保护电 图 MIC5332电源管理电 图 IO以及电机电 图 电源接 图 电 图 FM25V01器电 图 MAX3051CAN总线电路设 图 TXS0108电路设 图 电机驱动电 图 USB接口电路设 图 microSD卡槽电路设 图 GPS与安全开关接 图 图 主板上的MPU9250电路设 图 主板上的HMC5983电路设 图 主板上的MS5611电路设 图 IMU减震模块上的L3GD20电路设 图 IMU减震模块上的LSM303D电路设 图 IMU减震模块上的MPU6000电路设 图 IMU减震模块上的MS5611电路设 图 典型主从机电路连 图 SPI通讯协议数据传输示意 图 角度测量模 图 角速度测量模 图 罗盘处于水平状态的坐标 图 FMU主的JATG接 图 烧录BootLoader的连线示意 图 ST-LinkUtility界 图 控制台刷固件示意 图 无人机地面站刷固件示意 图 飞控的启动流 图 飞控系统的应用程序框 图 板载程序结构 图 系统顶层的任务管理框 图 地面站的通讯架 图 姿态估计及控制的典型实现过 图 主处理器结构 图 协处理器结构 图 地理坐标系与机体坐标系示意 图 姿态信息测量基本原 图 姿态解算过 图 完整的姿态控制框图（俯仰与横滚、偏航控制细节一致 图 倾转分离控制示意 图 位置估计过 图 位置控制过 图 输出混控逻辑关系 图63飞行模式评估 图 IMU减震 图 主控 图 接口 图 主控板与接口板的接口示意 图 电源板与接口板的焊接示意 图 IMU减震板与主控板的连接示意 图 飞控接口板正面示意 图 飞控接口版示意 表列表表 接口电压范 表 SPI地址位格 表 SPI数据位格 表 MPU9250采样率分配寄存 表 MPU9250配置寄存 表 MPU9250输出配 表 MPU9250陀螺仪配置寄存 表 MPU9250加速度计配置寄存 表 MPU9250加速度计测量值缓 表 MPU9250加速度计量程设 表 MPU9250温度测量值缓 表 MPU9250陀螺仪测量值缓 表 MPU9250陀螺仪量程设 表 MPU9250磁力计测量值缓 表 MPU9250磁力计配 表 飞行前健康检查概 表 电源接口线 表 备用电源接口线 表 I2C接口线 表 CAN接口线 表 数传接口线 表 GPS1接口线 表 GPS2接口线 表 调试串口接口线 表 JTAGIO接口线 表 JTAGFMU接口线 表 Spektrum接口线 表 蜂鸣器、USB接口线 表 电机接口线 引项目研发背景载荷。无人机诞生的直接原因是由于需求，可以追溯到第一次时期，线路所经区域多为高山大岭，运维不断增加，而线路运维并没有随着线以，在现场完成简单的组装测试，便可操作无人机沿着所在区域的输电目前云台的拍摄精度已达到10米外销钉级，完全满足线路故障排查所需要的要求。如此以来，的工作量得到极大的减少，最重要的是，将无人机应用国内外研究概况DraganflyerDraganflyer是RC玩具公司开发的一款商业产品，它由无线和控制微控制器来处理这些数据。目前的Draganflyer还安装了四个红外热传感模图 Draganflyer四轴飞行斯坦福大学（Stanford）研制的四轴飞行StanfordDrangonflyer飞行器进行了改进，设计应用于其自主飞行器控制研究（STARMAC）的测试平台。Stanford大学将原来Draganflyer四轴飞主要实现了姿态估测和数据通讯；在控制端则设计了PC和操作。惯性测GPS数据，速度数据，以及高度信息一起，融合计算出飞行器的位置和速度信息。飞行器如图2。图 宾夕法尼亚大学（UPenn）研制的四轴飞行宾夕法尼亚大学的研究通过使用HMX-4这一商业模型来开发了自己程PC通过对传感器传输的数据进行分析处理然后回送控制信息；其四轴控制相对的位移改变，地面PC即可计算出四轴飞行器的空间旋转角度和水平位移；3图 系统中PC端用于接收并处理由机发送过来的图像信息，通过这些信息统通过姿态传感器数据来维持自身的稳定，同时通过相应的获取PC端Quattrocopter四Quattrocopter是德国研究探索的一种微型无人机概念，是EADS公司设计在城市情况下可以垂直上下飞行的0.5千克无人飞行器。飞行器设计了RC接收部分来实现控制，同时控制系统采用了MEMS惯性测量传感器、GPS定位接收装置来确保飞行器的运行稳定；其电源部分使用一2565cm500g，其中一半的功率可用来承载负重，机身架构如图4所示。AR.Drone

图 Quattrocopter外形AR.DroneParrot2010iOSandroid设备控制。飞行器控制系统中具备超声波测距传感器、广角头传感器和MEMS姿态传感器，再结合强大的微处理器进行和姿态处理，使得AR.Drone可用具备较强的自动增稳性能；并且该飞行器可用使用wifi进行远拟现实的射击类应用。其结构如图4所示。大疆精灵系

图 AR.drone飞行器大疆的一家商业无人机公司。2016年3月2日，大疆发布了其44最大的特点是引入了“计算机视觉”与“机绕过物，从而大幅降低飞行器碰撞概率。但这并非大疆首创，英特尔在去年IDF上就开始将3D实感头用在了无人机上实现物规避功能，在今CESYUNEEC（昊翔）无人机在树林里游刃iOSDJIGO应用程序中选择“智能跟随”模式并点击被跟随目标，精灵4便会在视野中自动扫描该对象，并开始跟随。出其它可通行的路线，避开沿途，自动飞向指令的方向。一些开源的四轴飞行

图 作，从算法和硬件模块上对四轴飞行器进行不断优化升级，并通过KKKK是一款使用atmel的8位处理器设计的四轴飞行器，其架构简单，只控制时需要一定的人为调整，因此其操作难度较大。KK四轴实现的控制代码较四轴者们也对KK四轴进行了大量改进，引入了加速度，磁力等姿态测量传感器，进一步提高四轴飞行器的飞行性能。KK四轴模型如图7所示：7KK71，2，3，4为四个电调接入端，KK四轴采用专门的电子调1，2，3，4则为PPM的四路控制信号接入，分别为roll、pitch、yaw和thro。MK（Mikrokopter）四MK是德国开发设计的一款基于atmel8位处理器的四轴飞行器，其系PCB功能板的形式来添其丰富的功能代码和模块化设计方式很有学习和借鉴价值。MK8图 MK四轴飞行器控制系板的形式将各个功能模块加载上去，涉及的模块板有：PPM板、GPS导航MWC（Multiwii）四MWC也是一款优秀的开源四轴飞行器，其拥有较多的开发者对其进行研究和改进，同时支持较多的硬件平台。MWCatmel8位处理器平台设计的，后来升级到32位处理器，同时拓展了对多传感器平台的支持，这使得MWC四轴飞行器的传感器使用算法具备学习借鉴意义。与MK不同，MWCPCB板上，也即整个控制系统集中在一起，使得MWC设计得更紧凑，体积更小，也更灵活。MWC四轴的控制系统如图9所示：9MWC9MWC控制系统板，机身以及电机等其他模块可以根据用户需要ArduPilotMega(APM)是市面上最强大的基于惯性导航的开源自驾仪。其硬KML格式。经过国外多个团队的联合开发，这款自驾仪拥有丰（"ArduPlane",,（"ArduRover"MAVLink语音和完整的带回放的数据记录；WindowsMacLinuxWindows下使用图形任务规划设(Mac下可用模拟器)或在任何操作系统下使用命令行界面。三种操作系统上都有可用的地面站程序。基于Arduino编程环境，也是完全跨系统的；完整支持Xplane和FlightGear半硬件仿真；ArduPilot的发展历史。图 ArduPilot历技术指 10HzGPS2GB及以上数据记录器，任务数据自动记录，并可以导出为bin的技术指标完成情况支持多旋翼（四旋翼，六旋翼，八旋翼，十旋翼等√√√√√√√√√√√10HzGPS√√2GB及以上的记录√√√√√√√项目的总体研究思路口组成，系统的整体框图如图11所示。接收指接收指传感器数传感器数据融飞行控辅助传感发出指飞行控辅助传感发出指微控制主循数据进行融合，并传输给控制器进而解算出给驱动装置的控制量。模块记录飞控板上搭载了一个擅长于强大运算的32bitSTM32F427VIT6CortexM4外加FPU（浮点运算单元）的168MHz/256KBRAM/2MBFlash处理器，STM32F427VIT6另有3个I2C，6个SPI，4个USART，2个CAN以及3路12bitADC，USBOTGHS/FS,CameraInterface，该1.7-3.6V供电，在主MCU上运行着NuttXRTOS实时操作系统；还有一个独立供电的主要定位与工业用途的协处理器32bit CortexM3的STM32F100C8T6B该的主要参数如下：2个SPI，2个I2C,3个USART，一个CAN，2路12bitADC，72MHz/128KBRAM/20KB，2.0-3.6供电。STM32F100C8T6B的特点就是安全稳定。所以就算主处理器死机了，还有一个协处理器来保障安全。本飞控可以输出14路传感传感伺服控PPMR/CDSMRSSIS.BUS安全开86外部I2Cx模拟信号模拟信号电池图 控制功能。针对检测仪各功能需求，采用如错误!未找到源。13所示的 驱图13飞控系统架驱中间件运行于操作系统之上，提供传感器的设备驱动和一个微对象请求（MicroObjectRequestBroker，英文简称uORB)用于飞控上运行的单个任系统硬件设计Pixhawk设计了一款飞控板，飞控的硬件一分为四：主板主板主要负责运算以及控制，主板上搭2MBFlash、256KBRAM的微控ST32F427VIT6128K板载的16KBSPIFRAM磁性随机器FM25V01，一套飞行器姿态测量传感器，包括九轴运动处理器MPU9250、气压计MS5611以及磁力计HMC5983；减震IMU板搭载各种传感器主要负责姿态测量，包括三轴加速度计和磁力计PPM输入，Spektrum/DSM，模拟 RSSI信号输入，S.Bus伺服输出，I2C、SPI、CAN、ADC、UART总线接口，压电蜂鸣器驱动以及安全开电路各部分提供一系列的EMI、ESD保护。主控板与接口板直接通过80pin的插IMU载板与主控板通过20pin的H11905CT-ND连接。考虑到飞控的减震需要，IMU载板上集成了FPC。可安全切换到备份控制，使用了一个32位ARMCortexM4微控制器STM32F427VIT6作为主处理器32位ARMCortexM3微控制器STM32F100C8T6B作为协处理器；传感器了也进行了冗余设置，飞控上搭载了IMU惯性测量单元，主板上一套，IMU2套，安装在主准备信号被路由；提供了大量的外设接口，包括UART，I2C，CAN，ADC；可以输出14路供舵机输出；采用多余度供电系统，可实现不间断供电，并且每个设备单独供电；使用了外置安全开关；全色LED智能指示灯；反应飞行器各种状态指示器，并集成了microSD卡控制器，可以进行高速数据记选TIM稳压输选TIM稳压输TIMADC4GSDIO+电压电电源管理设计电源是整个系统的，因此，电源设的坏直接影响系统的性及其稳85%以上的供电效率，大大减少了能源浪费。电源失效后自动二极管控制（不间断供电；支持最大10V舵机电源和最大10A障转移的理想二极管控制器舵机最大7V和高电流所有外设均有过流保护，ESD保护。采用多余度供电系统，可实现不间断供电和自动故障转图 电源管理模块电源管理模块从FMU中分离出来，另外组成了一块电源管理模块，具有低5V供给，集成了两块电源模块和相匹配的选择，包括电流和电压感应。为图 16中电源管理模块选用了凌力尔特公司（LinearTechnologyCorporation）LTC44172.5V36V系统。该控为1.5%的欠压和过压门限设定的窗口之内达到256ms时，此电源就被定义为个电源MIC5332，并进行电池的接驳和管理，由电池控制BQ24315从一个电池取出稳定的5V电源分别供给不同类的设备，防止互相干扰MIC5332的电源来自单独的BQ24313。两级处理保障了电源的可靠性。BQ24315和BQ24313都是高度集成的过压和过流保护，输入电流上图 BQ24313/5电源保护电上图中两个BQ24315都输入稳定的5V电压，分别输出VDD_5V_HIPOWERVDD_5V_PERIPHGPS、CAN总线以平正常，输出低电平说明电调电压过压同时切断OUT输出。FMUIO电源供FMUIO3.3V电压下运行，而且他们两个都有自己私有的双通道图 MIC5332电源管理电载的电压变化进行快速响应。从一路输入无延迟的分出稳定的输出电压。输入电压2.3V到5.5V，器。同时VOUT2为传感器单独供电。电USB或是通过电源模块接口或是第二电源模块接口传输给飞控，每一个电源都有翻转电极保护和从其他来的备用电能。伺服装置供能10VIO19IO上图所示BQ24313对于低于10.5V的输入电压输出电压限制在5.5V以内，VOUT2输出3.3V电压供IO协处理器使用。备用电供能图 电图 电用了一个OUT口单独供电。USB4–4–4–04–4–4–4-4–4–4–01EMI过滤，电源保护保证了传输给外接设备的电压不5.5V.电源会在合适的供给电压掉到2.7V以下或是上升到5.7V以上的时候断开连接。外接电源被分成两组11.5A电流限制，被用来给一个遥测电台供电这个输出是EMI过滤而且直接从USB/电源模块输入.这个端口的顶峰供电流过2A,这对于一个30dBm的在合理的效率下运行已经足够了.所有其他的外接设备共个1A的电流限制和一个单个的电源切换。在这个端口的顶峰电流不应该超过1.5A.每一组电源都是单独地在控制下切换.Spektrum/DSM,R/C接口是从它自己的校准器提取电能.这个端口在控制下被打开，然后Spektrum/DSM可以通电.Spektrum接收器大约能接收25mA的电量.S.Bus和CPPM直接从伺服轨道接收电能,对于备用电池，FMUIO微控需要给实时时钟SRAM供电。板子上的备用电池有充足的电能用于时钟和SRAM，它们供电给由于无意电量流失或是其FMU测可以量两块电源模块的电池电压和电流,5V不规律的供应轨道(用于检测掉电状态)。IO可以测量伺服于端口的持续短路导致损伤这个锁可以被FMU重新设置在掉电状态下，FMU低压／过载监视器提供了一个保持FMU重启耗电的供能。MCU一颗性能强劲的32位主控制器STM32F427ARMCortexM4外加32STM32F103可以，在主处理器失效时可实现手动恢复。主控STM32F427上连接了一块FM25V01128K非易失铁电器。这种器既有EEPROM的速度，又像Flash一样掉电不会丢失数据，一般用来图 FM25V01器电上图中，FM25V01STM32F427SPI（SerialPeripheralInterface,是通过SPI接口进行通讯的。SPI接口是Motorola首先全双工三线同步串行接口，采用主从模式（MasterSlave）架构；支持多slave模式应用，一SPI目前应用中的数据速率可达几Mbps的水平，这也是为什么选择了使用SPI总线进行连接而不是I2C的原因，I2C不能作为高速传感器总线。扩展接口电路设计MAX3051CAN协议控制器和控制局域网(CAN)总线物理连线的MAX3051适用于+3.3V供电、不受汽车工业(ISO11898)规范故障保护限制的应图 MAX3051CAN总线电路设对于USART总线接口，使用了TXSTXSTXS0108通用电平驱动，这里主要是起到信号和增强驱动能力的作用。这样做的好处是一旦某个串口出现大电流只能烧毁驱动，不会烧毁MCU。图 TXS0108电路设电机驱电机驱动模块使用了TXSTXSTXS0108通用电平驱动，这里主要是起烧毁驱动，不会烧毁MCU。25TXS0108EOEGND上，这样设计的目的是使得在上电以及断电过程中输出处于高阻态。VCCAVCCB，AB端不需要电平转换，只起到作用。IO8个6个继电器接口上述3.4节中的14路电机驱动电路除了用作电机输同时还可以向辅助通道USB接口电路设计飞控固件的以及校准主要通过microUSB来完成，使用了带EMI电磁干扰滤波和ESD静电保护的上行终端NUF2042XV6。26USB用，用这个口可以把USB线延长出来，这样在后期进行调试和时就不分线路最短，以缩短差分线走线距离,严格遵循对称平行走线的布线规则，这样SD图 microSD卡槽电路设GPS和安全开关STM32F100故障协处理器上，飞控中对安全开关的按钮作了图 GPS与安全开关接 蜂鸣鸣器的信号使用LT3469做运放处理。LT3469是Linear公司的一款具有升压型稳压器的压电微型致动器驱动5V或者12V输入电压放大到33V图 图29中在1角OUT上连接了一个5.6nF的电容，根据设计原理超传感器模块世界是三维的，飞行器的三维位置非常重要。比如客机飞行的时候，都因此本飞控中使用了相关的传感器来获取这些状态量，并且采传感器的16STMicroL3GD20陀螺仪，用于测量旋转速度；三14位加速度计和磁力LSM303D，用于确认外部影响和罗盘指向，可选择外部磁力计，在需要的时候可以自动切换；InvenSense16位加速度计和位陀螺仪MPU6000以及三轴16位加速度计、陀螺仪和磁力计MPU9250；三轴数字罗盘HMC5983，用于确认航向；MEASMS5611气压计，用来测量高度；GPSGPS需要减少干扰，一般安装在飞机的最置，因此本项目使用的外置GPS模块。主板传感器MPU9250作为一9轴的数字运动处理器，其包含了三轴16位加速度计、MPU6050+AK8963MPU9250用户可程式控制的加速度计全格感测范围为±2g、±4g、±8g与±16g，以及最的SPI。图 MPU9250X,Y,ZMEMS组成。利用科里奥16位的数据。各种速率（250,±500,±1000,or1000,或者±2000°/s）都可以被编程。ADC的采样速率也是可编程，从每秒3.9-8000个，用户还可以选择是否使用低通滤波器来滤掉多余的杂波。MPU9250的三轴加速度也是单独分开测量。根据每个轴上的电容来测量轴的偏差度。结构上降低了各种因素造成的测量偏差。当被置于平面时候，当被置于平面时候，它XY0g，Z1g的重力加速度。加速的输出。范围是通过编程可调±2g,±4g,±8g,或±16g。尔效应传感器，通过驱动电路，信号放大和计算电路来处理信号地磁场在X，Y，ZADC均可满量程（0±4800µT）16位霍尼韦尔三轴电子罗盘HMC5983是一个温度补偿的三轴集成电路罗盘，用于HMC118XASIC，还有放大器，自动消磁带、偏置带和一个12bitADC。测量航向角的精确度达到1-2度。在±8GS2mGS30℃至+85℃的工作环境温度。供电电压2.16V-3.6V。

图 主板上的HMC5983电路设1μA10-1200mbar的气压数值。MS5611SPII2C装、全金属外壳、集成24位高精度AD等特性，这些特性使其非常图 IMU载板传感具备2.4V-3.6V供电电压、-40℃至+85℃的工作环境温度、兼容I2C和SPI数字

图 IMU减震模块上的L3GD20电路设STLSM303DL3GD20L3GD20一同可以组成完整的9度（DegreeofFreedom，英文简称DOF）航姿传感器系统，并且其-40℃至+85℃的工作环境温度、兼容I2C和SPI数字接口、集成温度传感器，这些参数几乎与L3GD20一致。

图 IMU减震模块上的LSM303D电路设MPU6000633轴陀螺仪。的SPI。MPU-6000可在不压下工作，VDD供电电压为2.5V±5%、3.0V±5%3.3V±5%，逻辑接口VVDIO1.8V±5%。

图 IMU减震模块上的MPU6000电路设IMU载板上的MS5611电路连接与主板上的MS5611图 IMU减震模块上的MS5611电路设传感器冗余设置加速度的低频特性好，但是对于十分敏感，本飞控中采取了使加速度的解决方案，一个是MPU6000，第二个是LSM303D。LSM303D800Hz取样，而MPU6000的采样率为1kHz。只有极不寻常的情况模式的噪声才DCM代码涉及到匹配每个加速度计每个时间步长GPS速度向量和当前姿态，而对EKF，是基于协方差矩阵产生用于两个加速度计的。一块板，以X轴为，在一秒钟的时间转动了90度，那么它在X轴上的角速90（DPS,，DegreePerSecondS，体现了飞控上设置了两个GPS接口以防GPS故障，也可以让获得很多日志显示，即使两个相同GPS模块，在飞行GPS模块出现严重错误也是很GPS模块之间不断进行切换，基于锁定状态和星数以得到更好、更的GPS数据。供电部分电源模块支持最高达30V，5.3V输出，最高3A电流，采用开关稳DC-DC供电模块，同时带有电压电流检测SPI通信Interface，中文名为串行设备接口是Motorola公司推出的一种同步串行接术。SPI总线在物理上是通过接在设备微控制器(PICmicro)上面的微处理控制单元(MCU)上叫作同步串行端口(SynchronousSerialPort)的模块(Module)来实现的，它允许MCU以全双工的同步串行方式，与各种设备进SPIEEPROMFlash，实时时钟(RTC)，数模转换器(ADC)，数字以及数据传输，节约了的管脚数目，同时为PCB在布局上节省了空间。正是出于这种简单易用的特性，现在越来越多的上都集成了SPI技术。机的控制下产生。SS信号用于或使能外设的收发功能。SS为高电平时，禁止外设接收和发送数据；SS为低电平时，允许外设接收和发送数据。图37所示是微处理器通过SPI与外设连接的示意图。在本飞控中，每个传感器都是作为从设37SPI串行寄存器写入MOSI信号线将字节传送给从机，从机也将自己的移位寄存器中的内容通过MISO信号线返回给主机(如图示)。这38SPISPI接口的就以这种方式工作。在这种传输方式下，从机的片选端必须采用主-从模式(Master-Slave)SPI规定了两个SPI设备之间通信必须由主设备(Master)来控制从设备采用同步方式(Synchronous)传输数主设备会根据将要交换的数据来产生相应的时钟脉冲(ClockPulse)，时钟脉冲组成了时钟信号(ClockSignal)，时钟信号通过时钟极性(CPOL)和时钟(CPHA)控制着两个SPI设备间何时以及何时对接收到的数据进行采(DataSPI设备间的数据传输之所以又被称为，是因为SPI协议规定一SPI设备不能在数据通信过程中仅仅只充当一个“发送者(Transmitter)”或者主设备进行(Access)。所以，主设备必须首先通过SS/CS管脚对从设备进行片选，把想要的从设备选上。弃，导致SPI物理模块最终失效。因此，在程序中一般都会在SPI传输完数据后，去SPI设备里的数据，即使这些数据(DummyData)在的程序里是SPI16SPISPI数SPISPI

表 SPI地址位格表 MPU9250数据分下面以MPU9250为例对传感器到的数据处理进行一下说明MPU9250相应寄存器中存有原始数据加速度计测量AXAY、AZ，陀螺仪测量值GX、GY、GZ和磁力计测量值MX、MY、MZ，通过简单的算法之后可以得到Roll，Pitch，Yaw。4MPU9250SMPLRT_DIV8位无符号值，以陀螺仪为例，通过该值将陀螺仪输出分频，MPU9250

1+DLPF_CFG=0/78Khz1Khz。而且DLPF滤波频率一般设置为采样率的一半。采样率，假定设置为50Hz，么：SMPLRT_DIV=1000/50-1=19SMPLRT_DIV −1=表 MPU9250配置寄存（DLPFFSYNC信号状态。根据下面的表格定义的值，采表 MPU9250输出配寄存器27-陀螺仪配置表7 MPU9250陀螺仪配置寄存器ZGYRO_Cten设置此位，Z轴陀螺仪进行自我测试XGYRO_Cten、YGYRO_CtenZGYRO_Ctenbits可以激活陀螺仪对应轴的自自检的响应=打开自检功能时的传感器输出-未启用自检功能时传感器在MPU9250的电气特性表中已经给出了每个轴的限制范围。当自28表8MPU9250加速度计配置寄存器ax_st_en设置为1时，X轴加速度感应器进行自检。ax_st_en设置为1时，Y轴加速度感应器进行自检。ax_st_en设置为1时，Z轴加速度感应器进行自检。寄存器59到64（ACCEL_XOUT_H,ACCEL_XOUT_L,ACCEL_YOUT_H,ACCEL_YOUT_L,ACCEL_ZOUT_H,and表 这些寄存器加速感应器最近的测量值感应数据寄存器，都由两部分寄存器组成（类似于STM32F10X系列中的寄存器：一个寄存器，用户不可见。另一个用户可读的寄存器。寄存器中中的值到用户可读的寄存器中去，避免了直接对感应测量值的突发。FullScaleLSB016384181922409632048FullScaleLSB016384181922409632048表 值计算，如果设置了量程为±2g（ACCEL_FS_SEL=00），-32768~(216=32768)就代表了-250~+250，此时它的LSB（最低有效位）就是/=9.8m/s^2Rx=Ry、Rz的计算与Rx原理相同，此时就可以得到向量R（加速度计测量的力矢量）和X，Y，Z轴之间的角度𝐴𝑥𝑟，𝐴𝑦𝑟，𝐴𝑧𝑟。图 回到加速度模型，感的是向量R（加速度计测量的力矢量）和XY、Z轴之间的角度，将它们定义为𝐴𝑥𝑟，𝐴𝑦𝑟，𝐴𝑧𝑟R到真正的现实加速度计的x轴、y轴、z轴.）组成的直角三角形：cosX=cos(𝐴𝑥𝑟)=cosY=cos(𝐴𝑦𝑟)=cosZ=cos(𝐴𝑧𝑟)=Rz/𝑅R=√𝑅2+𝑅2+𝑅2 满足R2=𝑅2+𝑅2+𝑅2得到重力向量与各个轴的夹角

(

)== (𝑅𝐴𝑦𝑟= 其中𝑅2+𝑅2+𝑅2

(

)=片在制作时都不一样，上的都的值，真正的在水平时Z轴以及-1g时的读数，得到一个补偿值，在每次ADC结果后都进行补偿。这ADC𝑥=𝐾∗𝐺𝑥+传感器的输出；K表示标度因数。寄存器65、66–温度传感器测量值（TEMP_OUT_H表 TEMPC=

TEMPOUT−RoomTempOffset

)+GYRO_YOUT_L,GYRO_ZOUT_H,and表 40MPU9250也就是检测围绕各个轴转动的速度。像三轴的陀螺仪将同时检测X、Y、Z轴的旋转。由上面这个模型图，首先定义：𝑅2=𝑅2+ 𝑅2=𝑅2+ Z𝑅𝑥𝑧、𝑅𝑦𝑧之间的夹角𝐴𝑥𝑧、𝐴𝑦𝑧。其中𝐴𝑥𝑧表示𝑅𝑥𝑧Z夹角，𝐴𝑦𝑧表示𝑅𝑦𝑧和Z轴间的夹角。设，已经测量围绕Y轴的旋转角（这将是𝐴𝑥𝑧角）在时刻𝑡0，其定义为𝐴𝑥𝑧0，接下来，测量这个角度是在稍后的时间𝑡1是𝐴𝑥𝑧1。变化率将被计算Rate𝐴𝑥𝑧=(𝐴𝑥𝑧1−𝐴𝑥𝑧0)/(𝑡1−如果𝐴𝑥𝑧单位是度，并以秒为时间单位，那么Rate𝐴𝑥𝑧将以度每秒(deg/s)表示。但是MPU9250并不会以deg/s单位输出，需要在读完后进行转换。先来表 通过SPI接口读出来的转换结果ADC值，并不是以度每秒为单位。一般按以AnglerateADCrate/以量程为±1000º/s为例，说明如何转换。假设X轴的ADC值为200,从上表中得知在±1000º/s下的灵敏度为32.8LSB/(º/s)。根据上面的公式：Anglerate=200/32.8=这就是说,MPU9250检测到模块正在以约6度每秒的速度绕X轴(或者称为在YZ平面上ADC值并不都是正的,请注意,当出现负数时,意味着该设备从现有表 MPU9250MPU605015MPU9250MODE[3:00000”：断电模式；“0001”：磁力计单“0010”：持续测10011”：持续测量模0100”：外部触发测量模式其他模式都是被的。X，Y轴在水平面内，X轴为前进方向，YX轴向右，ZX轴的夹角即为方位角α。再计算方位角。俯仰角φZ轴与的分量分别为𝐴𝑥,𝐴𝑦,XZ图 θ=arcsin[𝑔φ=arcsin[𝑔此时，磁传感器测出的地磁场在罗盘三轴方向的向量为𝐻𝑥,𝐻𝑦,𝐻𝑧,由坐标关𝐻𝑋=𝐻𝑥𝑐𝑜𝑠𝜃−𝐻𝑌=𝐻𝑥𝑠𝑖𝑛𝜑𝑠𝑖𝑛𝜃+360−[𝑎𝑟𝑐𝑡𝑎𝑛

)]

𝐻>0,𝐻>180−[𝑎𝑟𝑐𝑡𝑎𝑛

𝑌)]

𝐻>0,𝐻>𝛂

−[𝑎𝑟𝑐𝑡𝑎𝑛 )]

𝐻< 𝐻𝑋=0,𝐻𝑌< 𝐻𝑋=0,𝐻𝑌>位角A。当所在地区磁偏角β已知时，真北方位角为A=α+系统设飞控的包括BootLoader应用两部分组成。BootLoader采用面象的结构化编程方法，主要用于处理器的升级。应用的软NuttX+ROMFS构成。分为三大部分：实时操作系统、中间件和飞行控制栈。BootLoader和应用中的优先级是相互独立的，Bootloader是系统在上电后执行的第一段代码，通过这段小程序，其CPU和相关硬件的初始化之后，再将操作系统映像或固在的。检测不到端口的话，无法使用控制台给飞控板烧写固件，使用控设置，飞控板无法正常使用。因此需要先进行两个的Bootloader烧录。图 FMU主的JATG接飞控板上预留了的JTAG接口，使用JST_SUR_H6封装，由于SWD模SWDSWD模式仅使用两条最终接线情况如图43。图 BootLoader图 将飞控板连入，将px4fmu-v2_bl.bin文件拖入到界面内，擦除（Erase）erify烧写。图44所示即为成功效果。对于STM32F100C8T6B从处理器的BootLoader烧录与STM32F427方法相同，但需要先用ST-Link2连接STM32F1JTAG接口并选择px4io_bl.bin文件。完成主控STM32F427与故障协处理器STM32F100的BootLoader的烧USB连接到电脑就可以在端口中看到相应的接口了，显示为“PX4FMU”。文件烧录到飞控中，上传成功的结果如图45。45站的“固件”界面拔插一次USB，地面站会自动获取服务器上的飞控固件并烧录的界面如图46。图 编译编译时将Firmware/cmake/configs/nuttx_px4fmu-v2_default.cmake文件配置的模块地面站配置会在SD卡中生成/fs/tdprms各类信息（机架类型，校准信息，飞行模式等）飞控上电，执行Bootloader的main_f4、main_f1（分别对应着FMU和main判断是否升 ，不升级则直接进入操作系_start是系统的复 ，负责 的底层初始化，包括时钟、GPIO等单元的初始化，并通过串口显示启动状stm32_boardinitialize负责初始化板上资源，包括设置启动电压，配置 选取以及USB的初始化os_sart启动NutX，负责OS的底层初始化，包括各种队列和进程结构的初始化o_rnup创建初始化任务并启动系统，负责OS基本进程的启动以及用户进程的启动用户启动 由CONFIG_USR_ETRPONT宏进行指定FMU中的#defineIO中的#defineuser_start负责IO基础环境的初始化，包 、串口、nsh_main中的nsh_initscript执 就是等资源的初始化，最后进入一个死循环，用于入，与FMU通信的输 挂载SD执行rc.io,erface（混控器执行rc.mc_apps（飞控位姿算法user_start中的负责处括的输入内容SBUS的处理①IO收到FMU的串口数据后运行4747BootLoadermain_f4还有main_f1，分别对应着FMU主和故障协处理器IO。如果不升级芯片则进入操作系统(即飞控系统的应用)，进行板载资源的初始化以及操作系应用结合具体使用需求考虑，本飞控选用了NuttXARM系统，NuttX是一个实时操作系统（EmbeddedRTOS强调标准兼容和小型封装，具有从fork()采用来自Unix和常RTOS（VxWorks）的额外的标准API。新的处理器架构、SoC架构或板级架构，可以构建为开放的、平面的RTOS，或单独构建为系统调用接口的微内核，NuttXPOSIX/ANSI的任且NuttX是在BSD证下发布的，这也是最终选择NuttX作为本飞控的在本飞控中，NuttX作为最底层，以提供POSIX-style的用户操作环境(如printfpthreads/dev/ttyS1,open(),write(),poll(),ioctl())，进行底层的任务调度，同时提供硬件驱动、网络通信、UAVCAN和故障保护（failsafe）系统。中间中间件运行于操作系统之上，提供设备驱动和一个微对象请求（MicroObjectRequestBrokerμORB）用于驾驶仪上运行的单个任务之间的uORB通过被命名的总线（buses，在此处被称作话题(topic)）交换信息。在飞控系一个包含姿态结构体（横滚、俯仰、偏航）的信息传送出去。节点可以在话题/订阅（publish/subscrib）uORB实现的。本飞控选用NuttXARM系统uORBNuttX而言，它仅仅是一个普通的文件设备对象，这个设备支持Open、Close、Read、Write、Ioctl以及Poll机制。通过什么时候收到这条消息。它只需要单纯的把要广播的数据推送到uORB的消息uORB是本飞控系统中非常重要的且关键的一个模块，它肩负了整个系统的数据传输任务，所有的传感器数据、GPS、PPM信号等都要从获取后通过uORB进行传输到各个模块进行计算处理。在这里uORB实际上就是一套跨进飞控中，一个仅包含一种消息类型，通俗点就是数据类型。每个进程可以订阅（subscribe）或者发布（publish），可以存在多个发布者，或者一个进程 publish(publish(48uORB实际上是多个进程打开同一个设备文件，进程间通过此文件节点进行uORB的实现是通过重载CDev基类来组织一个uORB的设备实例。并且完uORB的启动参数来完成对应的功能，uORBstart/test/status3条启动参数，在飞控的rcS启动中，使用start参数来进行初始化，其他2个参数分别用来进行uORB功能的自检和列出uORB的当前状态。在rcS中使用start参数启动uORB后，uORB会创建并初始化它的设备实例，其中的实现大部分都在CDev基类完成。这个过程类似于Linux设备驱动中的飞行控制栈④控制器输出部分：混控输出和执行器，限幅。系统的板载程序结构图如图49。图 49中每个框表示一个概念上的任务。图中不是所有模块都默认使能的，系统中的各个模块都进行了高度模块化设计，系统的顶层架构如下行。每一个箭头都是一种通过uORB进行发布/订阅调用的连接。并且本飞控的50与飞控硬件配套的，本项目也设计了一款地面站QGroundControl，地径数据，可以是位置坐标，可以是无人机的各种数据等等。地面站的作在本系统中，无人机与地面站之间的交互是通过一种商业逻辑应用如指令应用（一般命令与控制，例如，导航应用（接受务将其为层航的原始数据）mvlinkmvlinkINK数据包并将其转级uB。式构晰可免Vink对系统的深度依赖。ink应用也获取了大量的传感器数据和状态估计值，并将其发送到地面站，下图所示为地面站的通讯架构。51现，根据飞机器的不同，当需要一个特定的模型控制器的时候，其中一些模块也可以组一个单独的应用。姿态姿姿态姿态估位置机器视位置动动力电电机图 处理器的设主处理器的设传传感器数姿态原始波后数飞行器姿态滤波后PID控制量数数飞行器模原始数PID控制数滤波后数据PID控制飞行器姿态飞行器协处信日志模式姿态图 图53所示为本飞控的主处理器FU结构图传感器进程的信程取据状识进结合器据飞器态识出行当模，最姿控进程过行姿、据、飞行器模式计算出当前所需的PID控制量并推送至uOB，协处理器通信进程再将订PID控制量通过高速串口发送至协处理器。NYNNYN设 脉YN获 定时器20ms中断->YNt图 54IOPPMDMA方式直接填充至CPU20ms中断中实现，每次进中断会DMA传输计数器的值并记录，再与上次的记录值进会出现失去一个度的情况。四元数法则不存在这个问题，并且运算步骤也相飞控算法设计取了飞行器当前的姿态与位置之后就可以使用姿态控制算法与位置控制算法对飞行器进行精确控制了；这其中还包括了混控输出与导航控制。姿态解算算法设计机体坐标系，其原点𝑂𝑏取在飞行器的重心上，坐标系与飞行器固连（通过用欧拉角来表示，就是公式+系数。比如欧拉角公式和欧拉角的系数（滚转、俯仰、偏航。姿态的数据来源有5个：重力、地磁、陀螺仪、加速度计、磁力计。图 航向解滤磁力积滤陀螺姿态解滤加速度航向解滤磁力积滤陀螺姿态解滤加速度图 加速度计测量的是重力分量，有参照物即重力轴，因此在无外力的加加速磁加速磁角速+校正后的角速5757所示为本飞控的姿态解算算法过程，采用的是互补滤波的方法。姿态姿态控制算法设计RPY的实现不再赘述。控制对象就是四旋翼飞行器，其动力𝑚𝑥=𝑢(𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜓+𝑚𝑦=𝑢(𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓−𝑚𝑧=𝑢𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜙−𝜓=𝜃= =系统动力学模型：J𝑤̇𝑏+𝑤̇𝑏𝑥𝐽𝑤̇𝑏=𝜏如姿态增量，从而达到控制无人机姿态的作用。PID控制是运用最多的PPIDPID算法。内环控制姿态角速度，外图 当前坐标 中间坐标 目标坐标仅俯仰-偏航（倾斜 仅偏航（旋转图 图59中红色的是飞行器当前的坐标系，蓝色的是要经过转换得到的目标坐X轴横滚或绕Y轴俯仰）转换到绿色的中间坐标系，然后绕Z轴旋转一个角度比控制偏航更容10°的变-俯仰需要60ms左右，但是偏航控制器却需要近150ms。原因是对于“X”型结构的多旋翼来说，要实现位置估计算法设计+++-+积++位60图60为本飞控的位置估计算法流程。整个算法的思想是由地理坐标系位置控制算法设计目标位置目标位置+--内环多旋对速度测量信 位置测量信61P-PIDPPID是速度控PID速度控制器，得到可利用的推力设输出混8个设计相同类型动作的输出组混控：混控是由一套独立的定标器（用来归一化数值、器组成的，88个输出属于同一个物理输出设备，如FMU板或者IO板。它代表混控的输出。混控油门等信号；控制组1执行对机身的控制，输出飞行器降落信号；控制组2是对62中混控器的作用是根据预定好的规则和参数将一系列输入整合为一系化或者限制为一个范围之内，因此输入给混控器的数字值范围都在-1.01.0之飞行器的几何外观（X型、+型、六旋翼、Y型等电机的输出死区（限制范围横滚、俯仰、偏航的输入范围为-1.0~1.0，而油门的输入范围为0~1.0。为了 1进行了限制（100757575），飞行器将出现翻滚或者不稳定的在