基于MPU6050的INS惯性导航和实时姿态检测系统

1、基于MPU6050的INS惯性导航和实时姿态检测系统1 .项目目标及功能说明11项目目标学习使用正点原子探索者开发板，并熟悉开发板上的MPU6050六轴传感器的工作原理和各函数的调用过程。同时学习开发板的扩展接口，尝试在开发板上扩展蓝牙模块，并实现开发板与手机等含有蓝牙模块的电子设备通过蓝牙连接并进行数据的传输。在完成上述内容的基础上，实现将MPU6050六轴传感器的加速度计和陀螺仪的数据传送到手机上，在手机上实现陀螺仪的变化效果展示。同时通过串口将MPU6050数据传送到电脑上，通过Matlab编程处理数据，实现惯性导航的简单展示。12系统功能说明系统最主要的功能有两个：一个是在手机端能够展

2、示开发板上MPU6050陀螺仪的姿态变化，通过一个立方体的转动来表示陀螺仪的转动：另一个是在电脑端能够读取MPU6050的数据，并通过对数据的处理还原数据中存储的MPU6050的姿态变化，简单展现出惯性导航的效果。在实现系统最主要的两个功能过程中，还需要实现一些基础功能。开发板能够通过蓝牙与手机连接并传输数据：开发板能够通过串口将数据发送出去；在电脑端能够读取开发板上串口输出的数据等。2 .需求分析 惯性导航系统用于各种运动机具中，包括飞机、潜艇、航天飞机等运输工具及导弹，然而成本及复杂性限制了其可以应用的场合。但是，存在一种情形:卫星一旦突然因故障、敌方打击或干扰（如太阳风暴）等原因无法提供

3、服务，这对依赖GPS、北斗等卫星导航系统作为唯一PNT（PositionXNavigation.Time）信息来源的系统来说可能是致命的灾难。作为目前为止卫星导航系统最好的备援一一惯性导航系统（INS）,将于届时发挥出巨大的作用，其精度完全可以媲美GPS等卫星导航系统。并且它不需要外部参考就可确定当前位置、方向及速度，从而使它自然地不受外界的干扰和欺骗。定位、导航和授时服务对军队而言就像氧气对人类一样不可或缺，因此通过研究新机理、研制新设备、开发新算法，以摆脱人员和系统设备对GPS的依赖,具有极大的战略意义。 姿态监测系统可广泛应用丁关键资产姿态变化的无线实时监控。由于目前移动智能终端设备的数

4、量和质量逐步提升，因此，通过计算机上传统的上位机软件进行姿态监测，逐渐暴露出了自身的缺点一一串口传输无法实现无线监测、计算机相比智能终端便携性极差。因此，使用无线传输（蓝牙、红外、WIFI、GSM等）的技术，开发一款在移动智能终端可以实时显示物体姿态的应用，具有很高的实用价值和广泛的市场应用前景。移动终端操作系统:计算机操作系统:串口开发:开发板IDE：AndroidIDE：3 .开发环境Android4.4.4KitKatWindows8.1Prox64MATLABR2014aKeiluVisionsEclipseJavaEEIDEforWebDevelopersAndroidDevelop

5、mentToolkit4685184 .项目进展情况到目前为止，我组已实现了以下功能：1 .STM32F4开发板上MPU6050六轴传感器的数据获取并显示在LCD屏幕上。2 .在LCD屏幕上绘出圆形图案，且圆形图案能根据MPU6050六轴传感器的姿态变化而运动，传感器倾斜角度越大，图案运动速度越快。3 .扩展蓝牙模块，能通过蓝牙模块与手机连接并进行数据通信。4 .根据函数提供的帧格式定义数据帧，并通过USART接口将数据帧传给PC端。5 .在手机端能根据蓝牙获取的MPU6050六轴传感器的陀螺仪数据绘出立方体，立方体能在可接受的时间延迟内实时展现MPU6050的姿态变化（转动方

6、向和角度）。6 .在PC端能通过对从USART接口获取的数据帧进行解析获取MPU6050加速度传感器和陀螺仪的数据，并根据数据帧中设置的校验位进行数据校验。7 .在PC端能根据解析出的加速度传感器和陀螺仪数据，在可接受的误差范围内还原MPU6050的姿态变化（包括位移、转动方向和角度），实现一个简单的惯性导航系统。5.系统设计5.1 HC总线工作原理5.1.1 总线的构成及信号类型12c总线是由数据线SDA和时钟SCL构成的串行总线，可发送和接收数据。在CPU与被控IC之间、IC与IC之间进行双向传送，最高传送速率lOOkbpSo各种被控制电路均并联在这条总线上，但就像电话机一样只有拨通各自的

7、号码才能工作，所以每个电路和模块都有唯一的地址，在信息的传输过程中，I2C总线上并接的每一模块电路既是主控器（或被控器），乂是发送器（或接收器），这取决于它所要完成的功能。CPU发出的控制信号分为地址码和控制量两部分，地址码用来选址，即接通需要控制的电路，确定控制的种类；控制量决定该调整的类别（如对比度、亮度等）及需要调整的量。这样，各控制电路虽然挂在同一条总线上，却彼此独立，互不相关。I2C总线在传送数据过程中共有三种类型信号，它们分别是：开始信号、结束信号和应答信号。开始信号：SCL为高电平时，SDA由高电平向低电平跳变，开始传送数据。结束信号：SCL为低电平时，SDA由低电平向高电平跳变

8、，结束传送数据。应答信号：接收数据的IC在接收到8bit数据后，向发送数据的IC发出特定的低电平脉冲，表示已收到数据。CPU向受控单元发出一个信号后，待受控单元发出一个应答信号，CPU接收到应答信号后，根据实际情况做出是否继续传递信号的判断。若未收到应答信号，由判断为受控单元出现故障。这些信号中，开始信号是必须的，结束信号和应答信号都可以不要，IIC总线时序图如图5.L1-1所示。SDAOUTSCLSDAIN.叩AAhHkjHxxxxxxxxxxxxxx/x1图5.1-1HC总线时序图探索者STM32F4开发板板载的EEPROM芯片型号为24co2。该芯片的总容量为256字节，通过IIC总线与

9、外部连接。STM32F4开发板有硬件IIC,但是设计的比较复杂，而且稳定性不好，所以我组使用GPIO软件模拟IIC未对24C02进行读写。同时使用软件更具有移植性，只要有10口，将软件移植过去就能使用模拟的lie,而硬件必须MCU的支持。5.1.2 硬件设计IIC_SCLIIC-SDAPB8139PB9140实现模拟HC需要用到的硬件资源有：串口（USMART）、GPIO、24C02。PB8/TIM4_CH3ZI2C1_SCL/DCNn_D6PB9/TIM4CH4/I2C1SDA/DCMID742S2WSU14AOAlA2GNDVCC1WPSCLSDA87C81VCC3.3lieSCLGND6

10、2lieSDAIR65tllHVCC3324C02GND图5.1-2STM32F4与24C02连接图我组通过GPIO来模拟HC,24c2的SCL和SDA分别连在GPIO_PB8和GPIO_PB9上，连接关系如图5.1.2-lc5.2 MPU6050工作原理5.2.1 MPU6050弓脚TopViewRES<CPOUTRES<RES<SCLSDAFSYNCREGOUT>8VLOGIC>cxCL图5.2-1MPU6050结构图模块外观如图521-2所示:图5.2-2MPU6050实物图INT(6ADO(SDO>SCLUSCLK)SDA.(SDI)AUX.DAFS

11、YNCCPOUTVDOGNDREGOUT(VLOGIC)图5.2-3MPU6050内部逻辑框图如图5.2.1-1为MPU6050六轴传感器的结构图，总共有24个引脚，而图5.2.1-2为MPU6050的内部逻辑框图，描述了MPU6050内部的模块结构，以及各引脚的连接情况。表521-1MPU6050引脚输出和信号描述PinNumberMPU-6050PinNamePinDescription1YCLK1NOptionalexternalreferenceclockinput.ConnecttoGNDifunused6YAUX_DAI2Cmasterserialdata,forconnectin

12、gtoexternalsensors7YAUX_CLI2CMasterserialclock,forconnectingtoexternalsensors8/CSSPIchipselect(0=SPImode)8YVLOGICDigitalI/Osupplyvoltage9ADO/SDOI2CSlaveAddressLSB(ADO);SPIserialdataoutput(SDO9YADODCSlaveAddressLSB(ADO)10YREGOUTRegulatorfiltercapacitorconnection11YFSYNCFramesynchronizationdigitalinpu

13、t.ConnecttoGNDifunused12YINTInterruptdigitaloutput(totempoleoropen-drain)13YVDDPowersupplyvoltageandDigitalI/Osupplyvoltage18YGNDPowersupplyground19,21YRESVReserved.Donotconnect.20YCPOUTChargepumpcapacitorconnection22YRESVReserved.Donotconnect.23SCL/SCLKI2Cserialclock(SCL);SPIserialclock(SCLK)23YSCL

14、I2Cserialclock(SCL)24SDA/SDII2Cserialdata(SDA);SPIserialdatainput(SDI)24YSDAI2Cserialdata(SDA)2.3,4,5,14,15,16,17YNCNotinternallyconnectedMaybeusedforPCBtracerouting表521-1对每一个引脚的名称和作用进行了说明。在上述引脚中，SCL和SDA是连接MCU的IIC接口，MCU通过这个IIC接口来控制MPU6050。另外还有一个IIC接口，连接的引脚为AUXJZL和AUX_DA,这个接口可用来连接外部从设备，比如磁传感器，这样就可以g&

15、#39;MPU6050组成一个九轴传感器。VLOGIC是10口电压,该引脚最低可以到1.8V,我们一般直接VDD即可。ADO是从口接口（接MCU）的地址控制引脚，该引脚控制IIC地址的最低位，如果接GND,则MPU6050的IIC地址是0X68：如果接VDD,则是0X69。注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）。在探索者STM32F4开发板上，ADO是接GND的，即MPU6050的HC地址是0X68（不含最低位）。5.2.2硬件设计IIC_SCLI23UC-SDAI24R45VCC3.3R49GND10uFlC4SCLRESVSDARESVINTRESVAUX_CLA

16、UX_DACLKINFNYNCADOVLOGICVDDREGOUTGNDCPOUT191716B5_£TNIPU605010|C76|c77PB8/TIM4_CH3/I2C1_SCL/DCMI_D6PB9/TIM4CH4/I2C1SDA/DCMID7/I2S2WSPCO/OTGHSLTLPISTP/ADC123IN10图5.2-4MPU6050与STM32F4的连接电路图从图5.2.2-1可以看出，MPU6050通过三根线与STM32F4开发板连接，其中IIC总线时和24C02以及WM8978共用，接在PB8和PB9上面。MPU6050的中断输出，连接在STM32F4的PCO脚，不过

17、本例程我们并没有用到中断。另外，ADO接的GND,所以MPU6050的器件地址是：0X68。5.2.3 初始化操作在使用STM32F4读取MPU6050的加速度和角度传感器数据之前，需要做以下初始化操作：(1)初始化HC接口MPU6050采用IIC与STM32F4通信，所以我们需要先初始化与MPU6050连接的SDA和SCL数据线。(2)复位MPU6050这一步让MPU6050内部所有寄存器恢复默认值，通过对电源管理寄存器1(0X6B)的bit7写1实现。复位后，电源管理寄存器1恢复默认值(0X40),然后必须设置该寄存器为0X00,以唤醒MPU6050,进入正常工作状态。(3)设置角速度传感

18、器(陀螺仪)和加速度传感器的满量程范围这一步，我们设置两个传感器的满量程范围(FSR),分别通过陀螺仪配置寄存器(0X1B)和加速度传感器配置寄存器(0X10设置。我们一般设置陀螺仪的满量程范围为±2000dps,加速度传感器的满量程范围为±2g。(4)设置其他参数这里，我们还需要配置的参数有：关闭中断、关闭AUXIIC接口、禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器(DLPF)等。本章我们不用中断方式读取数据，所以关闭中断，然后也没用到AUXIIC接口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器(0X38)和用户控制寄存器(0X6A)控制。MPU60

19、50可以使用FIFO存储传感器数据，不过本章我们没有用到，所以关闭所有FIFO通道，这个通过FIFO使能寄存器(0X23)控制，默认都是0(即禁止FIFO),所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器(0X19)控制，这个采样率我们一般设置为50即可。数字低通滤波器(DLPF)则通过配置寄存器(0X1A)设置，一般设置DLPF为带宽的1/2即可。(5)配置系统时钟源并使能角速度传感器和加速度传感器系统时钟源同样是通过电源管理寄存器T(0X1B)来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是0(内部8MRC震荡),不过我们一般设置为1，选择X轴陀螺PLL作为时钟源，以获得

20、更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器2(0X6C)来设置，设置对应位为。即可开启。5.2.4 相关寄存器在读取MPU6050数据中主要用到了以下寄存器：(1)PowerManagement1(电源管理寄存器1)表5.2.4-1电源管理寄存器1各位描述Register(Hex)RegisterBft7BitsBit5Bit4Bit3Bit2BiMBitO68107DEVICE_RESESLEEPCYCLETEMP.DISCLKSEL(2:0如表5.2.4-1,寄存器地址为0x6B。DEVICE_RESET位用来控制复位，设置为1,复位MPU6050,

21、复位结束后，MPU硬件自用清零该位：SLEEEP位用于控制MPU6050的工作模式，复位后，该位为1,即进入了睡眠模式(低功耗)，所以我们要清零该位，以进入正常工作模式：TEMP_DIS用于设置是否使能温度传感器，设置为0,则使能：CLKSEL2:0用于选择系薪时钟源，选择关系如表5.24-2所示，默认是使用内部8MRC晶振的，精度不高，所以我们一般选择X/Y/Z轴陀螺作为参考的PLL作为时钟源，一般设置CLKSEL=001即可。表5.2.4-2CLKSEL选择列表CLKSEL(2:«000内部8MRC晶振001PLL.使用X轴陀螺作为参考010PLL,使用Y轴能作为叁考011PLL

22、.使用Z轴能作为参考100PLL.使用外部32.768Khz作为参考101PLL,使用外薄19.2Mhz作为参考110保留111关闭时钟.保持时序产生电路复位状态（2） GyroscopeConfiguration(陀螺仪配置.寄存器)表524-3表陀螺仪配置寄存器各位描述Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4B*3Bit2Bit1BitO1B27XG.STYG.STZG.STFS_SEL1:t*如表5.24-3,寄存器地址为OxlB。FS_SEL1:O两个位用于设置陀螺仪的满量程范围：0为±250。/s；1为±500&

23、#176;/s；2%±1000“/s；3为±2000。/s。我们一般设置为3,即±2000°/s,而因为陀螺仪的ADC为16位分辨率，所以得到灵敏度为：65536/4000=16.4LSB/(°/s)«（3） AccelerometerConfiguration(力口速度传感器配置寄存器)表524-4加速度传感器配置寄存器各位描述Register(Hex)RegisterBit7Bit6Bit5Bit4Bit3BK2BitlBitO1C28XA.STYA.STZA.STAFS_SEL(1:0如表524-4,寄存器地址为OxlC。AFS

24、_SEL1:O两个位用于设置加速度传感器的满量程范围：0为±2g：1为±4g;2%±8g；3为±16g°我们一般设置为0,即±2g,而因为加速度传感器的ADC也是16位分辨率，所以得到灵敏度为：65536/4=16384LSB/go（4） FIFOEnable(FIFO使能寄存器)表524-5FIFO使能寄存器各位描述Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2BitlBftO2335TEMP_FIFO.ENXG_FIFO.ENYG_FIFO.ENZG_FIFO.ENAC

25、CEL_FIFO_ENSLV2_FIFO_ENSLV1_FIFO_ENSLY).FIFO.EN如表5.24-5,寄存器地址为0x23。该寄存器用于控制FIFO使能，在简单读取传感器数据的时候,可以不用FIFO,设置对应位为0即可禁止FIFO,设置为1,则使能FIFO。注意加速度传感器的3个轴,全由1个位(ACCEL_FIFO_EN)控制,只要该位置1,则加速度传感器的三个通道都开启FIFO了。（5） SampleRateDivider(采样率分频寄存器)表524-6陀螺仪采样率分频寄存器各位描述Register(Hex)Register(Decimal)Bit7BitsBit5Bit4Bit3

26、Bit2BK1BitO1925SMPLRT_DI47：0如表5.24-6,寄存器地址为0x19。该寄存器用于设置MPU6050的陀螺仪采样频率，计算公式为：采样频率=陀螺仪输出频率/(1+SMPLRT_DIV)这里陀螺仪的输出频率，是IKhz或者8Khz,与数字低通滤波器（DLPF）的设置有关，当DLPF_CFG=0/7的时候，频率为8Khz,其他情况是IKhz。而且DLPF滤波频率一般设电为采样率的一半。我们假定设置采样率为50Hz,那么SMPLRT_DIV=1000/50-l=19。（6） Configuration（配置寄存器）表5.24-7配置寄存器各位描述Register(Hex)R

27、egister(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1BitO1A26*EXT_SYNC.SET12:ODLPF_CFG2:0)如表S.2.4-7,寄存器地址为OxlA。加速度计和陀螺仪是根据数字低通滤波器（DLPF）的三个设置位进行过滤的，即DLPF_CFG2:0。DLPF_CFG不同配置对应的过滤情况如表524-8所示：表5.24-8DLPF_CFG配置表DLPF.CFGR:*加速度传感物Fs=1Khz角速度传感器（陀螺仪）Mm延迟（ms）鲁宽（Hz）延迟（ms）Fs(Khz)00026002560.9880011842.01881.91010943.0M

28、2.81011444.9424.81100218.52。8.311011013.81«13.41110519.0518.61111保窗保留8加速度传感器输出速率（Fs）固定是IKhz,而角速度传感器的输出速率（Fs）,则根据DLPF.CFG的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半,如前面所说的，如果设置采样率为50Hz,那么带宽就应该设置为25Hz,取近似值20Hz,就应该设置DLPF_CFG为100。（7） PowerManagement2（电源管理寄存器2）表5.2.4-9电源管理寄存器2各位描述Register(Hex)Register(Decimal)B

29、it7Bit6Bit5B<4Bit3Bit2Bit1BitO6C108LP_WAKE_CTRU1:0STBY_XASTBYYASTBY_ZASTBY-XGSTBY_YGSTBY_ZG如表5.249,寄存器地址为0x6C。该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率，项目中没有用到。剩下的6位，分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式，由于不需要进入待机模式，所以全部设置为0。（8） GyroscopeMeasurements（陀螺仪数据输出寄存器）表5.2.4-10陀螺仪数据输出寄存二器各位描述Register(Hex)Register(Decirml)Bit

30、7BitsBit5Bit4Bit3Brt2Bit1BitO4367GYRO_XOUT(15：«14468GYR0.XOUTP：04569GYRO_YOUT(15:»4670GYR0.YOUT7:04771GYRO_ZOUT(15：»4872GYRO_ZOUTI7:0如表524-10,总共有6个寄存器，地址为0x43-0x48,通过读取这6个寄存器，就可以读到陀螺仪x/y/z轴的值，比如x轴的数据，可以通过读取0x43（高8位）和0x44（低8位）寄存器得到，其他轴以此类推。（9） AccelerometerMeasurements（加速度传感器数据输出寄存器）表5

31、24.11加速度传感器数据输出寄存器各位描述Register(Hex)Register(Decimal)Bit7Bit6Bit5Bit4Bit3Bit2Bit1BitO3859ACCEL_XOUTI15:83C60ACCEL_XOUH7：03D61ACCEL_YOUT15:8J3E62ACCEL_YOUT(7:03F63ACCEL_ZOUTI15:84064ACCEL_ZOUT7:OJ如表524-11,总共有6个寄存器,地址为0x3B如X40,通过读取这8个寄存器，就可以读到加速度传感器x/y/z轴的值，比如读x轴的数据，可以通过读取0x3B（高8位）和0x3C（低8位）寄存器得到，其他轴以此

32、类推。5.3ATK-HC05工作原理5.3.1 ATK-HC05引脚ATK-HC05模块非常小巧（16mm*32mm）,模块通过6个2.54mm间距的排针与外部连接，模块外观如图531-1所示：ATK-HCX)5-V13一_ALIENTEK图53-1ATK-HC05模块外观图图5.3.L1中,从右到左,依次为模块引出的PIN1-PIN6脚，各引脚的详细描述如表531-1所示：表531-1ATK-HC05模块各引脚功能描述序号名称说明1LED配对状态,出；配对成功，出高电平，未配对网愉出低电平2KEY用于进入AT状态：高电平有效（悬空默认为低电平）.3RXD模块单口接收脚（TTL电平，不能X换按

33、RS232电平;）,可按单片机的TXD4TXD模块申口发送JU（TTL电平，不能直接接RS232电平;）可接单片机的RXD5GND地6VCC电源(3.3V-5.0V)另外，模块自带了一个状态指示灯：5加。该灯有3种状态，分别为：1 .在模块上电的同时（也可以是之前），将KEY设置为高电平（接VCC）,此时STA慢闪（1秒亮1次），模块进入AT状态，且此时波特率固定为38400o2 .在模块上电的时候，将KEY悬空或接GND,此时STA快闪（1秒2次），表示模块进入可配对状态。如果此时将KEY再拉高，模块也会进入AT状态，但是S7A依旧保持快闪。3 .模块配对成功，此时STA双闪（一次闪2下，2

34、秒闪一次）。53.2硬件设计PlVCC3J.BZ0520MBW20VCCCARTTXD1UIUART2VCC3313|_aGNDHC05UARTRXDUARTCTSUART-RTSPCMCLKpcM_ourKiClNPCM-SYNCAIOOAIOIRESETVCCJ.3GKD会感P1011ROWPIO9PIOSHO?PQ6PIO5PIO4PIO3P(O2HOInooGNDGNDRXDITLBSKEYBSLEDRT9193-33gSdGO510LEDVCC3323GNDBSKEYESLED图53-2ATK-HC05蓝牙串口模块原理图模块与单片机连接最少只需要4根线即可：VCC、GND、TXD、R

35、XD、VCC和GND用尸给模块供电,模块TXD和RXD则连接单片机的RXD和TXD即可。该模块兼容5V和3.3V单片机系统，所以可以很方便的连接到系统里面去。ATK-HC05模块与单片机系统的典型连接方式如图532-2所示：图53-3ATK-HC05模块与单片机系统连接示意图图中虚线连接表示可有可无，可以根据需要，选择性的使用。5.3.3ATK-HC05与蓝牙主机连接首先，开机检测ATK-HC05蓝牙模块是否存在，如果检测不成功，则报错。检测成功之后，显示模块的主从状态，并显示模块是否处于连接状态，DSO闪烁，提示程序运行正常。按KEYO按键，可以开启/关闭自动发送数据（通过蓝牙模块发送）；按

36、KEY_UP按键可以切换模块的主从状态。蓝牙模块接收到的数据，将直接显示在LCD上（仅支持ASCII字符显示）。同时，还可以通过USMART对ATK-HC05蓝牙模块进行AT指令查询和设置结合手机端蓝牙软件（蓝牙串口助手vl.97.apk）,可以实现手机无线控制开发板（点亮和关闭LED1）O所要用到的硬件资源如下：1. 指示灯DSO、DS12. KEYO/KEY_UP两个按键3. 串口1、串口34. TFTLCD模块5. ATK-HC05-V13蓝牙串口模块前面介绍了ATK-HC05蓝牙串口模块的接口，而AUENTEK探索者STM32F407开发板板载了一个ATK模块接口（ATKMODULE）

37、,ATK-HC05蓝牙模块可直接插入该接口实现与探索者STM32F4开发板的连接。ATKMODULE同开发板主芯片的连接原理图如图5.3.3-1所示：pJsART31戈PB1069|坐ART3乂PB1170后BCLE©3DINTPC026PB10/SP12_SCK；T1M2_CH3OJ3_TX1：PB11/TIM2_CH4/U3_RXZI2C2_SDA/I£PF6|BCKe£U7PIORS232/BTCOM&GPSATKMODULEVCC5cm>旦axxaxH(N)uo>U3TXUSART3R>T1GBCJDfJcm30AH"o

38、n。,XIO8PI产I,PF6/TIM10CH1/?SMCNIORD/ADC3IN4图53-4ATK-M0DULE接口与MCU连接关系从上图可以看出，蓝牙模块的串口最简单的办法是连接在开发板的串口3上面，只需要用跳线帽短接P10的USART3_RX和GBC_TX以及USART3_TX和GBC_RX即可实现。连接好之后，探索者STM32F407开发板与ATKHC05蓝牙模块吊连接关系如表2.1所示：表533-1ATK-HC05蓝牙模块同探索者STM32F407开发板连接关系表ATK-HC05蓝牙模块与开发板连接关系ATK-HC05蓝牙串口模块VCCGNDTXDRXDKEYLED探索者STM32F

39、407开发板5VGNDPB11PB10PF6PCO使用时，我们只需要将ATK-HC05蓝牙模块插入到开发板的ATK-MODULE接口即可，如图533-2所示：图53-5ATK-HC05蓝牙模块与探索者开发板对接实物图注意，连接好之后，记得检查P10的跳线帽：必须短接:USART3_RX和GBJDC以及USART3JX和GBC_RX。另外，在实际使用的时候，如果不需要进入AT设置和状态指示，则连接底牙模块只需要4根线连接即可：VCC/GND/TXD/RXDoATK-HC05模块可以与多种蓝牙主机设备连接，这里仅以智能手机为例，进行说明。首先，在手机上安装：BTCIient.apk,该应用可以在提

40、供的资料里面找到。安装完应用后，我们打开该应用，如图533-3所示：韶园于0-|Qw|EIRTYLJ1°|p分词m符123XICIVIBINM图5.3-6BTCIient.apk运行界面进入搜索蓝牙设备界面，如图533-4所示：图53-7搜索蓝牙设备从上图可以看出，手机己经搜索到蓝牙模块了，HC-05,点击这个设备，输入默认密钥1234（仅笫一次连接需要设置），完成配对，如图533-5所示：88e0a.图53-8输入配府密码在输入密码之后，等待一段时间，即可连接成功，如图5.33-6所示:图53-9连接成功此时，手机和蓝牙模块就连接上了，手机便可以接收数据并绘制三维图形了,改变开发板

41、的姿态，手机端绘制的三维图形如图533-7所示：图53-10手机绘制三维图形界面这样，我们就实现了ATK-HC05模块与手机的连接。同其他蓝牙主机设备的连接，方法都是类似的，比较简单，这里我们就不再介绍了。有了S1A指示灯，我们就可以很方便的判断模块的当前状态，方便使用。5.4 DMP算法原理通过陀螺仪数据输出寄存器和加速度传感器数据输出寄存器获取的是MPU6050加速度传感器和陀螺仪的原始数据，而实验需要的是姿态数据，即欧拉角:航向角(Yaw)、横滚角(Roll)和俯仰角(Pitch)。要得到欧拉角数据，就得利用我们的原始数据，进行姿态融合解算，这个比较复杂，知识点比较多，而MPU6050自

42、带了数字运动处理器，即DMP,并且,InvenSense提供了一个MPU6050的嵌入式运动驱动库，结合MPU6050的DMP,可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到Yaw、Roll和Pitcho使用MPU6050的DMP输出的四元数是q30格式的，也就是浮点数放大了2的30次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以2的30次方，然后再进行计算，计算公式如下：其中quat0quat是MPU6050的DMP解算后的四元数,为q30格式,所以要除以一个2的30次方，其中q30是一个常量为1073741824,即2的30

43、次方，然后带入公式，计算出欧拉角。上述计算公式的573是弧度转换为角度，即180/IT,这样得到的结果就是以度(°)为单位的。q0=quat0/q30;q30格式转换为浮点数ql=quatl/q30;q2=quat2/q30;q3=quat3/q30;计算得到俯仰角/横滚角/航向角Pitch=asin(-2*ql*q3+2*q0*q2)*57.3;俯仰角Roll=atan2(2*q2*q3+2*q0*ql,-2*ql*ql-2*q2*q2+1)*573；横滚角Yaw=atan2(2*(ql*q2+q0*q3)zqO*qO+ql*ql-q2*q2-q3*q3)*57.3;航向角5.5

44、AHRS5.5.1 AHRS简介AHRS称为航姿参考系统，包括多个轴向传感器，能够为飞行器提供航向,横滚和侧翻信息，这类系统用来为飞行器提供准确可靠的姿态与航行信息。PitchRoll图5.5-1航姿参考系统航姿参考系统包括基于MEMS的陀螺仪，加速度计和磁强计。航姿参考系统与惯性测量单元IMU的区别在于，航姿参考系统（AHRS）包含了嵌入式的姿态数据解算单元与航向信息，惯性测量单元（IMU）仅仅提供传感器数据，并不具有提供准确可靠的姿态数据。目前常用的航姿参考系统（AHRS）内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。航姿参考系统（AHRS）具有两个主要特点：（1）高精度36

45、0度全方位位置姿态输出，但采用欧拉角的会具有万向锁，不能全向转动。（2）高效的数据融合算法快速动态响应与长时间稳定性（无漂移，无积累误差）相结合。航姿参考系统（AHRS）输出模式为三维全姿态数据（四元数/欧拉角/旋转矩阵），5.5.2 AHRS软件设计AHRS的软件设计主要分为:传感器初始化，包括设置传感器的更新速率、最程。初始化卡尔曼滤波的相关矩阵，根据传感器的特点设置过程激励噪声协方差矩阵Q,设为对角元素为0.1的四维对角方阵。若成功读取陀螺仪数据，进行卡尔曼流波的时间更新。采集加速度传感器和磁阻传感器的数据，若读取成功则进行观测更新。加速度观测更新与磁场观测更新算法差别在观测方差的R.可

46、根据两种传感器的置信度没置相应的值，航向姿态参考系的程序流程如图5.5.2-1所示。初始化传博器观测更新成功读取花-螺仪数据2/继续进、行EKF就波?成功读取虻场加速度计?侦测更新图5.5-2航向姿态参考系的程序流程5.6 手机端功能原理及Android代码对手机端apk的修改主要是揩两个独立的安卓apk进行了源码级的合并。5.6.1 蓝牙帧处理蓝牙模块方面，使用了开源的蓝牙串口通讯代码。在进行合并的过程中，主要对代码的接收模块进行了修改。由于蓝牙的发送和接收以及Android广播中传递的均为字符串，因此，为了解析方便，我们将蓝牙帧设置为如下格式：u3printf(HP56c%.lfR56c%

47、.IfY56c%.lfn"/cp/Pitch/cr/Roll/cy/Yaw);其中，cp、cr>cy分别为Pitch、Roll、Yaw的符号，为非负数时显示为空格符。这里注意到在进行格式化输出的时候，控制小数点后保留一位小数，但是小数点前的位数无法控制，猜测是由于c语言头文件功能较为精简造成的。5.6.2 参数的广播传递为了实现参数传递的功能，查阅了大量Android相关的代码和资料，最终选择了通过Android特有的广播机制进行参数的传递。此外，在广播的过程中，还是用了意图(Intent),这也是Android特有的一个机制，用来对广播进行定向生成和接收，保证三维制图端收到的

48、数据为蓝牙接收到并进行广播的数据。另外在对蓝牙数据进行接收时，发现无法理想地收取到单个帧数据，大部分时候是一次性获取到了几十帧的数据，其中笫一帧和最后一帧往往是不完整的,这就要求我们对数据进行处理之后再进行广播：接收数据线程while(true)num=is.read(buffer);读入数据n=0;StringsO=newString(buffer,O,num);fmsg+=sO;保存收到数据for(i=0;i<num;i+)if(bufferi=0x0d)&&(bufferi+l=0x0a)buffer_newn=0x0a;i+；elsebuffer_newn=buf

49、ferfi;)n+;)Strings=newString(buffer_new,O,n);smsg+=s;写入接收缓存toSend+=s;写入广播缓存Stringtmp=toSend.splitC，n");判断换行符if(toSend.Iength()>=21)for(intlen=0;len<tmp.length;len+)if(tmplen.length()>=20)intent.putExtraf'CCSendflag",tmplen);intent.setAction("CCSendflag")；设置广播的action只

50、有和这个action一样的接收者才能接收广播；sendBroadcast(intent);发送广播)toSend=""处理完成，清空广播缓存if(is.available()=O)break;短时间没有数据才跳出进行显示广播接收类publicclassTestReceiverextendsBroadcastReceiverprivatestaticfinalStringtag="flag"privatestaticStringmessage="null”；publicvoidonReceive(Contextcontextjntentinten

51、t)Stringstring=intentgetstringExtra(1,CCSendflagH);接收指定广播iffstring!="")setString(string);)publicvoidsetStringfStringstr)message=str;)publicStringgetStringf)returnmessage;)5.6.3 视图切换在合并过程中，由于涉及到视图的切换，因此对Android操作系统的进程管理和线程创建进行了学习，最终能够实现从蓝牙接收视图到三维绘制视图的平滑切换。切换的时机最终选择在蓝牙连接建立后，开始接收数据的瞬间：打开接收线程t

52、ryis=_socket.getlnputStream();得到蓝牙数据输入流catch(IOExceptione)Toast.mdkeText(this,”接收数据失败！Todst.LENGTH_SHORT).show();return;)if(bThread=false)ReadThread.start();bThread=true;Intentintent=newIntentlBTClient.this,mainActivit/.class);intentputExtra("CCO,/smsg);startActivity(intent);elsebRun=true;)5.6.

53、4 OpenGL绘三维绘图方面，使用了官方提供的示例代码，主要在每一帧绘制的过程中加入了参数传递的功能，从而实现了实时从蓝牙接收数据并绘制三维图形:TestReceivertestReceiver;protectedvoidinit(GL10gl)gl.glClearColorfO.Of,0.0f,O.Of,l.Of);gl.glEnable(GL10.GL_DEPTH_TEST);gl.glEnable(GL10.GL_CULL_FACE);gl.glClearDepthf(l.Of);gl.glDepthFunc(GLlO.GLLEQUAL);gl.glClearDepthf(l.Of)g

54、l.glShadeModel(GL10.GL_SMOOTH);实例化接收类testReceiver=newTestReceiver);)publicvoidonDrawFrame(GL10gl)/TODOAuto-generatedmethodstubgLglClear(GL10.GL_COLOR_BUFFER_BIT|GL10.GL_DEPTH_BUFFER_BIT);获取广播的数据StringallString=testReceiver.getstringf);时广播数据进行解析，从字符串转化为float类型if(allString.length()>=20)Pitch=FloatparseFloat(allString.substring(allString.indexOf(,P:,)+2,allString.indexOf(,lR:,)-l);RolkFloatparseFloatlallString.substringtallString.indexOff'R?'J+Z,allString.indexOf(HY:M)-l);Yaw=FloatparseFloat(allString.substring(allString.indexOf(,Y:,)+2,allString.lengthf);)gl.glMatrixMode(GL10.GL_M