1. 项目目标及功能说明
1.1 项目目标
学习使用正点原子探索者开发板，并熟悉开发板上的MPU6050六轴传感器的工作原理和各函数的调用过程。同时学习开发板的扩展接口，尝试在开发板上扩展蓝牙模块，并实现开发板与手机等含有蓝牙模块的电子设备通过蓝牙连接并进行数据的传输。在完成上述内容的基础上，实现将MPU6050六轴传感器的加速度计和陀螺仪的数据传送到手机上，在手机上实现陀螺仪的变化效果展示。同时通过串口将MPU6050数据传送到电脑上,通过Matlab编程处理数据,实现惯性导航的简单展示.
1.2 系统功能说明
系统最主要的功能有两个:一个是在手机端能够展示开发板上MPU6050陀螺仪的姿态变化,通过一个立方体的转动来表示陀螺仪的转动；另一个是在电脑端能够读取MPU6050的数据，并通过对数据的处理还原数据中存储的MPU6050的姿态变化,简单展现出惯性导航的效果。
在实现系统最主要的两个功能过程中,还需要实现一些基础功能。开发板能够通过蓝牙与手机连接并传输数据；开发板能够通过串口将数据发送出去;在电脑端能够读取开发板上串口输出的数据等。
2. 需求分析
惯性导航系统用于各种运动机具中,包括飞机、潜艇、航天飞机等运输工具及导弹，然而成本及复杂性限制了其可以应用的场合。但是，存在一种情形： 卫星一旦突然因故障、敌方打击或干扰（如太阳风暴）等原因无法提供服务，这对依赖GPS、北斗等卫星导航系统作为唯一PNT（Position、Navigation、Time）信息来源的系统来说可能是致命的灾难.
作为目前为止卫星导航系统最好的备援——惯性导航系统（INS)，将于届时发挥出巨大的作用,其精度完全可以媲美GPS等卫星导航系统。并且它不需要外部参考就可确定当前位置、方向及速度，从而使它自然地不受外界的干扰和欺骗。 定位、导航和授时服务对军队而言就像氧气对人类一样不可或缺，因此通过研究新机理、研制新设备、开发新算法，以摆脱人员和系统设备对GPS的依赖，
具有极大的战略意义。
姿态监测系统可广泛应用于关键资产姿态变化的无线实时监控。由于目前移动智能终端设备的数量和质量逐步提升，因此,通过计算机上传统的上位机软件进行姿态监测，逐渐暴露出了自身的缺点——串口传输无法实现无线监测、计算机相比智能终端便携性极差。
因此，使用无线传输(蓝牙、红外、WIFI、GSM等）的技术，开发一款在移动智能终端可以实时显示物体姿态的应用，具有很高的实用价值和广泛的市场应用前景。
3. 开发环境
移动终端操作系统: Android 4。4。4 KitKat 计算机操作系统:Windows 8.1 Pro x64
串口开发: MATLAB R2014a 开发板IDE： Keil uVision5
Android IDE：Eclipse Java EE IDE for Web Developers
Android Development Toolkit 23。0。 4.1468518
4. 项目进展情况
到目前为止，我组已实现了以下功能：
1. STM32F4开发板上MPU6050六轴传感器的数据获取并显示在LCD屏幕
上。 2. 在LCD屏幕上绘出圆形图案，且圆形图案能根据MPU6050六轴传感器的
姿态变化而运动，传感器倾斜角度越大，图案运动速度越快. 3. 扩展蓝牙模块,能通过蓝牙模块与手机连接并进行数据通信。
4. 根据函数提供的帧格式定义数据帧，并通过USART接口将数据帧传给PC
端. 5. 在手机端能根据蓝牙获取的MPU6050六轴传感器的陀螺仪数据绘出立
方体，立方体能在可接受的时间延迟内实时展现MPU6050的姿态变化（转动方向和角度）。 6. 在PC端能通过对从USART接口获取的数据帧进行解析获取MPU6050加速度传感器和陀螺仪的数据，并根据数据帧中设置的校验位进行数据校验。
7. 在PC端能根据解析出的加速度传感器和陀螺仪数据，在可接受的误差范
围内还原MPU6050的姿态变化（包括位移、转动方向和角度），实现一个简单的惯性导航系统。
5. 系统设计
5.1 IIC总线工作原理
5.1.1 总线的构成及信号类型
I2C总线是由数据线SDA和时钟SCL构成的串行总线，可发送和接收数据。在CPU与被控IC之间、IC与IC之间进行双向传送，最高传送速率100kbps。各种被控制电路均并联在这条总线上，但就像电话机一样只有拨通各自的号码才能工作，所以每个电路和模块都有唯一的地址，在信息的传输过程中,I2C总线上并接的每一模块电路既是主控器(或被控器)，又是发送器（或接收器）,这取决于它所要完成的功能。CPU发出的控制信号分为地址码和控制量两部分，地址码用来选址，即接通需要控制的电路，确定控制的种类;控制量决定该调整的类别(如对比度、亮度等）及需要调整的量.这样，各控制电路虽然挂在同一条总线上，却彼此独立，互不相关。
I2C总线在传送数据过程中共有三种类型信号，它们分别是：开始信号、结束信号和应答信号。
开始信号:SCL为高电平时，SDA由高电平向低电平跳变，开始传送数据。 结束信号：SCL为低电平时，SDA由低电平向高电平跳变，结束传送数据。 应答信号:接收数据的IC在接收到8bit数据后，向发送数据的IC发出特定的低电平脉冲，表示已收到数据.CPU向受控单元发出一个信号后，等待受控单元发出一个应答信号，CPU接收到应答信号后，根据实际情况做出是否继续传递信号的判断。若未收到应答信号，由判断为受控单元出现故障。
这些信号中，开始信号是必须的，结束信号和应答信号都可以不要,IIC总线时序图如图 5。1.1-1所示。
图5.11IIC总线时序图
探索者STM32F4开发板板载的EEPROM芯片型号为24C02.该芯片的总容量为256字节，通过IIC总线与外部连接.STM32F4开发板有硬件IIC，但是设计的比较复杂，而且稳定性不好，所以我组使用GPIO软件模拟IIC来对24C02进行读写。同时使用软件更具有移植性,只要有IO口，将软件移植过去就能使用模拟的IIC，而硬件必须MCU的支持。 5.1.2 硬件设计
实现模拟IIC需要用到的硬件资源有：串口（USMART)、GPIO、24C02.
图5.12STM32F4与24C02连接图
我组通过GPIO来模拟IIC,24C2的SCL和SDA分别连在GPIO_PB8和GPIO_PB9上，连接关系如图 5.1。2—1。
5.2 MPU6050工作原理
5.2.1 MPU6050引脚
图5.21MPU6050结构图
模块外观如图 5。2。1-2所示：
图5.2错误!未定义书签。MPU6050实物图
图5.2错误!未定义书签。MPU6050内部逻辑框图
如图 5。2.1-1为MPU6050六轴传感器的结构图，总共有24个引脚，而图 5.2。1-2为MPU6050的内部逻辑框图,描述了MPU6050内部的模块结构，以及各引脚的连接情况。
表5.2.11MPU6050引脚输出和信号描述
Pin Number 1 6 7 8 8 9 9 10 11 12 13 18 19，21 20 22 23 23 MPU—6050 Y Y Y Y Y Y Y Y Y Y Y Y Y Y Pin Name CLKIN AUX_DA AUX_CL /CS VLOGIC AD0 / SDO AD0 REGOUT FSYNC INT VDD GND RESV CPOUT RESV SCL /SCLK SCL Pin Description Optional external referenceclock input.Connect to GND if unused. I2Cmasterserialdata, forconnecting to external sensors I2CMasterserialclock, forconnecting to external sensors SPI chipselect （0=SPI mode) Digital I/O supplyvoltage I2CSlaveAddress LSB (AD0)； SPI serialdata output (SDO) I2CSlaveAddress LSB （AD0) Regulatorfiltercapacitorconnection Frame synchronizationdigital input. Connectto GND ifunused. Interruptdigital output (totempoleoropen—drain) Power supplyvoltageandDigital I/Osupply voltage Power supplyground Reserved.Donot connect. Charge pump capacitorconnection Reserved.Donot connect。 I2Cserialclock （SCL）; SPI serialclock (SCLK） I2Cserialclock (SCL) 24 24 2,3， 4, 5， 14， 15，16， 17 Y Y SDA / SDI SDA NC I2Cserialdata（SDA）; SPIserial data input (SDI) I2Cserialdata（SDA) Notinternallyconnected. May beusedforPCB tracerouting。 表 5。2。1—1对每一个引脚的名称和作用进行了说明.在上述引脚中，SCL和SDA是连接MCU的IIC接口，MCU通过这个IIC接口来控制MPU6050。另外还有一个IIC接口，连接的引脚为AUX_CL和AUX_DA,这个接口可用来连接外部从设备，比如磁传感器，这样就可以与MPU6050组成一个九轴传感器。VLOGIC是IO口电压，该引脚最低可以到1.8V，我们一般直接VDD即可.AD0是从IIC接口（接MCU）的地址控制引脚，该引脚控制IIC地址的最低位,如果接GND,则MPU6050的IIC地址是0X68；如果接VDD,则是0X69.注意：这里的地址是不包含数据传输的最低位的(最低位用来表示读写）。 在探索者STM32F4开发板上,AD0是接GND的，即MPU6050的IIC地址是0X68（不含最低位)。 5.2.2 硬件设计
图5.2错误!未定义书签。MPU6050与STM32F4的连接电路图
从图 5。2。2-1可以看出，MPU6050通过三根线与STM32F4开发板连接,其中IIC总线时和24C02以及WM8978共用，接在PB8和PB9上面。MPU6050的中断输出，连接在STM32F4的PC0脚，不过本例程我们并没有用到中断。另外，AD0接的GND,所以MPU6050的器件地址是:0X68.
5.2.3 初始化操作
在使用STM32F4读取MPU6050的加速度和角度传感器数据之前，需要做以下初始化操作：
(1) 初始化IIC接口
MPU6050采用IIC与STM32F4通信,所以我们需要先初始化与MPU6050连接的SDA和SCL数据线. (2) 复位MPU6050
这一步让MPU6050内部所有寄存器恢复默认值,通过对电源管理寄存器1(0X6B)的bit7写1实现.复位后，电源管理寄存器1恢复默认值（0X40),然后必须设置该寄存器为0X00，以唤醒MPU6050，进入正常工作状态。
(3) 设置角速度传感器（陀螺仪）和加速度传感器的满量程范围
这一步，我们设置两个传感器的满量程范围（FSR），分别通过陀螺仪配置寄存器（0X1B）和加速度传感器配置寄存器（0X1C）设置。我们一般设置陀螺仪的满量程范围为±2000dps，加速度传感器的满量程范围为±2g。 (4) 设置其他参数
这里，我们还需要配置的参数有:关闭中断、关闭AUX IIC接口、禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器(DLPF）等。本章我们不用中断方式读取数据，所以关闭中断，然后也没用到AUX IIC接口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器（0X38）和用户控制寄存器(0X6A）控制。MPU6050可以使用FIFO存储传感器数据，不过本章我们没有用到,所以关闭所有FIFO通道，这个通过FIFO使能寄存器(0X23）控制，默认都是0（即禁止FIFO），所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器(0X19）控制,这个采样率我们一般设置为50即可。数字低通滤波器（DLPF)则通过配置寄存器（0X1A）设置,一般设置DLPF为带宽的1/2即可。 (5) 配置系统时钟源并使能角速度传感器和加速度传感器
系统时钟源同样是通过电源管理寄存器1（0X1B）来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是0(内部8M RC震荡），不过我们一般设置为1，选择x轴陀螺PLL作为时钟源，以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器,这两个操作通过电源管理寄存器2（0X6C）来设置，设置对应位为0即可开启. 5.2.4 相关寄存器
在读取MPU6050数据中主要用到了以下寄存器： (1) Power Management 1（电源管理寄存器1）
表5.2.4错误!未定义书签。电源管理寄存器1各位描述 Register （Hex） 6B Register 107 Bit7 DEVICE_RESET Bit6 SLEEP Bit5 CYCLE Bit4 - Bit3 TEMP_DIS Bit2 Bit1 CLKSEL［2：0] Bit0 如表 5。2。4-1，寄存器地址为0x6B.DEVICE_RESET位用来控制复位，设置为1，复位MPU6050，复位结束后,MPU硬件自动清零该位；SLEEEP位用于控制MPU6050的工作模式,复位后,该位为1，即进入了睡眠模式（低功耗）,所以我们要清零该位，以进入正常工作模式；TEMP_DIS用于设置是否使能温度传感器，设置为0，则使能；CLKSEL[2：0]用于选择系统时钟源,选择关系如表 5。2.4-2所示,默认是使用内部8M RC晶振的，精度不高，所以我们一般选择X/Y/Z轴陀螺作为参考的PLL作为时钟源，一般设置CLKSEL=001即可。
表5.2.41CLKSEL选择列表 CLKSEL［2：0］ 000 001 010 011 100 101 110 111 时钟源 内部8M RC晶振 PLL,使用X轴陀螺作为参考 PLL，使用Y轴陀螺作为参考 PLL,使用Z轴陀螺作为参考 PLL，使用外部32.768Khz作为参考 PLL,使用外部19.2Mhz作为参考 保留 关闭时钟，保持时序产生电路复位状态 (2) Gyroscope Configuration(陀螺仪配置寄存器)
表5.2.42表陀螺仪配置寄存器各位描述 Register (Hex) 1B Register（Decimal） 27 Bit7 XG_ST Bit6 YG_ST Bit5 ZG_ST Bit4 Bit3 Bit2 - Bit1 - Bit0 FS_SEL[1：0] 如表 5。2.4—3，寄存器地址为0x1B.FS_SEL［1：0］两个位用于设置陀螺仪的满量程范围：0为±250°/s；1为±500°/s；2为±1000°/s；3为±2000°/s。我们一般设置为3,即±2000°/s，而因为陀螺仪的ADC为16位分辨率，所以得到灵敏度为:65536/4000=16。4LSB/（°/s）。
(3) Accelerometer Configuration（加速度传感器配置寄存器）
表5.2.4错误!未定义书签。加速度传感器配置寄存器各位描述 Register （Hex） 1C Register 28 Bit7 XA_ST Bit6 YA_ST Bit5 ZA_ST Bit4 Bit3 Bit2 Bit1 — Bit0 AFS_SEL[1:0] 如表 5.2.4-4，寄存器地址为0x1C。AFS_SEL[1:0］两个位用于设置加速度传感器的满量程范围：0为±2g；1为±4g；2为±8g；3为±16g.我们一般设置为0，即±2g，而因为加速度传感器的ADC也是16位分辨率，所以得到灵敏度为：65536/4=16384LSB/g。
(4) FIFO Enable(FIFO使能寄存器）
表5.2.4错误!未定义书签。FIFO使能寄存器各位描述 Register （Hex) Register (Decimal） Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 23 35 TEMP_ FIFO_EN XG_ FIFO_EN YG_ FIFO_EN ZG_ FIFO_EN ACCEL _FIFO_EN SLV2 _FIFO_EN SLV1 _FIFO_EN SLV0 _FIFO_EN 如表 5。2。4-5，寄存器地址为0x23。该寄存器用于控制FIFO使能，在简单读取传感器数据的时候，可以不用FIFO,设置对应位为0即可禁止FIFO，设置为1，则使能FIFO。注意加速度传感器的3个轴，全由1个位（ACCEL_FIFO_EN）控制，只要该位置1，则加速度传感器的三个通道都开启FIFO了。
(5) Sample Rate Divider（采样率分频寄存器)
表5.2.43陀螺仪采样率分频寄存器各位描述
Register (Hex） 19 Register （Decimal) 25 Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 SMPLRT_DIV[7：0］ 如表 5.2.4—6，寄存器地址为0x19.该寄存器用于设置MPU6050的陀螺仪采样频率,计算公式为：
采样频率 = 陀螺仪输出频率 / （1+SMPLRT_DIV）
这里陀螺仪的输出频率，是1Khz或者8Khz，与数字低通滤波器（DLPF）的设置有关，当DLPF_CFG=0/7的时候，频率为8Khz,其他情况是1Khz。而且DLPF滤波频率一般设置为采样率的一半。我们假定设置采样率为50Hz，那么SMPLRT_DIV=1000/50-1=19。
(6) Configuration（配置寄存器)
表5.2.4错误!未定义书签。配置寄存器各位描述 Register Register (Hex） (Decimal) 1A 26 Bit7 - Bit6 - Bit5 Bit4 Bit3 Bit2 Bit1 DLPF_CFG［2:0] Bit0 EXT_SYNC_SET［2:0] 如表5。2.4—7,寄存器地址为0x1A。加速度计和陀螺仪是根据数字低通滤波器（DLPF）的三个设置位进行过滤的,即DLPF_CFG[2：0］.DLPF_CFG不同配置对应的过滤情况如表 5。2。4—8所示：
表5.2.44DLPF_CFG配置表
加速度传感器 角速度传感器(陀螺仪) Fs=1Khz DLPF_CFG［2:0］ Fs(Khz带宽（Hz) 延迟（ms） 带宽(Hz) 延迟（ms） ） 000 001 010 011 100 101 110 111 260 184 94 44 21 10 5 保留 0 2。0 3。0 4.9 8。5 13。8 19。0 256 188 98 42 20 10 5 保留 0.98 1。9 2.8 4.8 8.3 13.4 18。6 8 1 1 1 1 1 1 8 加速度传感器输出速率（Fs)固定是1Khz,而角速度传感器的输出速率(Fs）,则根据DLPF_CFG的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半,如前面所说的,如果设置采样率为50Hz，那么带宽就应该设置为25Hz，
取近似值20Hz，就应该设置DLPF_CFG为100。
(7) Power Management 2(电源管理寄存器2)
表5.2.45电源管理寄存器2各位描述 Register （Hex) 6C Register (Decimal) 108 Bit7 Bit6 Bit 5 STBY _XA Bit 4 STBY _YA Bit 3 STBY _ ZA Bit2 STBY _ XG Bit 1 STBY _YG Bit 0 STBY _ZG LP_WAKE_CTRL［1:0] 如表5。2.4-9，寄存器地址为0x6C.该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率,项目中没有用到。剩下的6位，分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式，由于不需要进入待机模式，所以全部设置为0。
(8) Gyroscope Measurements(陀螺仪数据输出寄存器)
表5.2.46陀螺仪数据输出寄存器各位描述 Register（Hex） 43 44 45 46 47 48 Register (Decimal） 67 68 69 70 71 72 Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 GYRO_XOUT［15:8] GYRO_XOUT［7：0］ GYRO_YOUT[15：8] GYRO_YOUT［7：0] GYRO_ZOUT[15:8］ GYRO_ZOUT［7:0] 如表5.2.4-10，总共有6个寄存器，地址为0x43~0x48,通过读取这6个寄存器,就可以读到陀螺仪x/y/z轴的值，比如x轴的数据，可以通过读取0x43（高8位）和0x44（低8位)寄存器得到，其他轴以此类推。
(9) Accelerometer Measurements(加速度传感器数据输出寄存器）
表5.2.4错误!未定义书签。加速度传感器数据输出寄存器各位描述 Register (Hex) 3B 3C 3D 3E 3F 40 Register （Decimal) 59 60 61 62 63 64 Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 ACCEL_XOUT［15：8］ ACCEL_XOUT［7：0] ACCEL_YOUT［15：8] ACCEL_YOUT[7:0］ ACCEL_ZOUT[15:8] ACCEL_ZOUT［7：0］ 如表5。2。4—11，总共有6个寄存器，地址为0x3B~0x40，通过读取这8个寄存器,就可以读到加速度传感器x/y/z轴的值,比如读x轴的数据，可以通过读取0x3B（高8位)和0x3C（低8位）寄存器得到，其他轴以此类推。
5.3 ATK-HC05工作原理
5.3.1 ATK—HC05引脚
ATK—HC05 模块非常小巧(16mm＊32mm），模块通过6个2.54mm间距的排针与外部连接,模块外观如图 5.3.1-1所示:
图5.3错误!未定义书签。ATK—HC05 模块外观图
图 5。3.1-1中，从右到左，依次为模块引出的 PIN1～PIN6 脚，各引脚的详细描述如表5.3。1-1所示：
表5.3.11ATK-HC05模块各引脚功能描述
序号 1 2 3 4 5 6 名称 LED KEY RXD TXD GND VCC 说明 配对状态输出；配对成功输出高电平，未配对则输出低电平。 用于进入 AT 状态；高电平有效(悬空默认为低电平)。 模块串口接收脚(TTL 电平，不能直接接 RS232 电平!），可接单片机的 TXD 模块串口发送脚（TTL 电平，不能直接接 RS232 电平！），可接单片机的 RXD 地 电源（3。3V～5.0V） 另外，模块自带了一个状态指示灯:STA。该灯有 3 种状态,分别为： 1. 在模块上电的同时（也可以是之前）,将 KEY 设置为高电平（接 VCC）,
此时 STA 慢闪（1 秒亮 1 次），模块进入 AT 状态，且此时波特率固定为 38400. 2. 在模块上电的时候，将 KEY 悬空或接 GND，此时 STA 快闪（1 秒 2
次），表示模块进入可配对状态。如果此时将 KEY 再拉高，模块也会进入 AT 状态，但是 STA 依旧保持快闪。 3. 模块配对成功，此时 STA 双闪(一次闪 2 下，2 秒闪一次）。
5.3.2 硬件设计
图5.31 ATK—HC05 蓝牙串口模块原理图
模块与单片机连接最少只需要4根线即可:VCC、GND、TXD、RXD、VCC和GND用于给模块供电，模块TXD和RXD则连接单片机的RXD和TXD即可。该模块兼容5V和3。3V单片机系统，所以可以很方便的连接到系统里面去。
ATK-HC05模块与单片机系统的典型连接方式如图5。3.2-2所示：
图5.3错误!未定义书签。ATK-HC05模块与单片机系统连接示意图
图中虚线连接表示可有可无，可以根据需要，选择性的使用.
5.3.3 ATK-HC05与蓝牙主机连接
首先，开机检测ATK—HC05蓝牙模块是否存在,如果检测不成功，则报错。检测成功之后，显示模块的主从状态，并显示模块是否处于连接状态，DS0闪烁，提示程序运行正常。按KEY0按键,可以开启/关闭自动发送数据（通过蓝牙模块发送）；按KEY_UP按键可以切换模块的主从状态。蓝牙模块接收到的数据，将直接显示在 LCD上(仅支持ASCII字符显示)。同时，还可以通过USMART对ATK—HC05蓝牙模块进行 AT 指令查询和设置。结合手机端蓝牙软件(蓝牙串口助手v1.97.apk），可以实现手机无线控制开发板(点亮和关闭 LED1）。
所要用到的硬件资源如下： 1. 指示灯DS0、DS1 2. KEY0/KEY_UP两个按键 3. 串口1、串口3 4. TFTLCD模块
5. ATK—HC05-V13蓝牙串口模块
前面介绍了ATK-HC05蓝牙串口模块的接口，而ALIENTEK探索者STM32F407开发板板载了一个ATK模块接口(ATKMODULE），ATK—HC05蓝牙模块可直接插入该接口实现与探索者STM32F4开发板的连接。
ATK MODULE同开发板主芯片的连接原理图如图 5。3。3-1所示：
图5.32ATK-MODULE 接口与 MCU 连接关系
从上图可以看出，蓝牙模块的串口最简单的办法是连接在开发板的串口 3 上面，只需要用跳线帽短接 P10 的 USART3_RX 和 GBC_TX 以及 USART3_TX 和 GBC_RX 即可实现。连接好之后，探索者 STM32F407 开发板与 ATK—HC05 蓝牙模块的连接关系如表 2。1 所示：
表5.3.3错误!未定义书签。 ATK-HC05蓝牙模块同探索者STM32F407开发板连接关系表
ATK-HC05 蓝牙模块与开发板连接关系 ATK—HC05 蓝牙串口模块 探索者 STM32F407 开发板 VCC 5V GND GND TXD PB11 RXD PB10 KEY PF6 LED PC0 使用时，我们只需要将 ATK—HC05 蓝牙模块插入到开发板的 ATK-MODULE 接口即可，如图 5.3.3-2所示：
图5.33ATK-HC05 蓝牙模块与探索者开发板对接实物图
注意，连接好之后，记得检查P10的跳线帽:必须短接：USART3_RX和GBC_TX以及USART3_TX和GBC_RX.另外，在实际使用的时候,如果不需要进入AT设置和状态指示，则连接蓝牙模块只需要4根线连接即可：VCC/GND/TXD/RXD。 ATK—HC05模块可以与多种蓝牙主机设备连接，这里仅以智能手机为例，进行说明。
首先，在手机上安装：BTClient.apk，该应用可以在提供的资料里面找到。 安装完应用后,我们打开该应用,如图 5.3。3-3所示：
图5.3错误!未定义书签。 BTClient。apk运行界面
进入搜索蓝牙设备界面,如图 5。3。3—4所示：
图5.3错误!未定义书签。搜索蓝牙设备
从上图可以看出,手机已经搜索到蓝牙模块了，HC—05，点击这个设备，输入默认密钥1234（仅第一次连接需要设置)，完成配对,如图 5。3.3—5所示：
图5.3错误!未定义书签。输入配对密码
在输入密码之后,等待一段时间，即可连接成功，如图5.3。3-6所示：
图5.34连接成功
此时，手机和蓝牙模块就连接上了，手机便可以接收数据并绘制三维图形了,改变开发板的姿态，手机端绘制的三维图形如图 5.3。3-7所示：
图5.35手机绘制三维图形界面
这样,我们就实现了ATK-HC05模块与手机的连接。同其他蓝牙主机设备的连接，方法都是类似的，比较简单，这里我们就不再介绍了。
有了 STA 指示灯，我们就可以很方便的判断模块的当前状态，方便使用。
5.4 DMP算法原理
通过陀螺仪数据输出寄存器和加速度传感器数据输出寄存器获取的是MPU6050加速度传感器和陀螺仪的原始数据,而实验需要的是姿态数据，即欧拉角：航向角（Yaw）、横滚角（Roll)和俯仰角（Pitch）。 要得到欧拉角数据，就得利用我们的原始数据，进行姿态融合解算，这个比较复杂，知识点比较多，而MPU6050自带了数字运动处理器，即DMP，并且,InvenSense提供了一个MPU6050的嵌入式运动驱动库，结合MPU6050的DMP,可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到Yaw、Roll和Pitch。
使用MPU6050的DMP输出的四元数是q30格式的,也就是浮点数放大了2的30次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以2的30次方,然后再进行计算，计算公式如下：
其中quat［0]～ quat[3］是MPU6050的DMP解算后的四元数，为q30格式，所以要除以一个2的30次方，其中q30是一个常量为1073741824,即2的30次方,然后带入公式，计算出欧拉角。上述计算公式的57.3是弧度转换为角度,即180/π，这样得到的结果就是以度（°）为单位的。
q0=quat[0］ / q30; //q30格式转换为浮点数 q1=quat［1] / q30； q2=quat［2] / q30； q3=quat［3] / q30; //计算得到俯仰角/横滚角/航向角 Pitch=asin（-2*q1＊q3 + 2*q0＊q2）＊57。3; //俯仰角 Roll=atan2（2*q2*q3 + 2*q0＊q1， -2＊q1*q1 - 2*q2＊q2 + 1)*57.3; //横滚角 Yaw=atan2（2*（q1＊q2 + q0＊q3）,q0＊q0+q1＊q1-q2*q2—q3*q3）＊57。3;//航向角 5.5 AHRS
5.5.1 AHRS简介
AHRS称为航姿参考系统，包括多个轴向传感器，能够为飞行器提供航向,横滚和侧翻信息，这类系统用来为飞行器提供准确可靠的姿态与航行信息。
图5.5错误!未定义书签。航姿参考系统
航姿参考系统包括基于MEMS的陀螺仪，加速度计和磁强计。航姿参考系统与惯性测量单元IMU的区别在于，航姿参考系统（AHRS）包含了嵌入式的姿态数据解算单元与航向信息，惯性测量单元（IMU）仅仅提供传感器数据，并不具有提供准确可靠的姿态数据.目前常用的航姿参考系统（AHRS）内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。
航姿参考系统(AHRS）具有两个主要特点:
(1) 高精度360 度全方位位置姿态输出，但采用欧拉角的会具有万向锁，不
能全向转动。 (2) 高效的数据融合算法快速动态响应与长时间稳定性（无漂移，无积累误
差）相结合。 航姿参考系统(AHRS）输出模式为三维全姿态数据（四元数/欧拉角/旋转矩阵）。 5.5.2 AHRS软件设计
AHRS的软件设计主要分为：
① 传感器初始化，包括设置传感器的更新速率、量程。
② 初始化卡尔曼滤波的相关矩阵,根据传感器的特点设置过程激励噪声协
方差矩阵Q,设为对角元素为0。1的四维对角方阵. ③ 若成功读取陀螺仪数据，进行卡尔曼滤波的时间更新。
④ 采集加速度传感器和磁阻传感器的数据，若读取成功则进行观测更新。
加速度观测更新与磁场观测更新算法差别在于观测方差的R,可根据两种传感器的置信度没置相应的值,航向姿态参考系的程序流程如图 5.5.2—1所示.
图5.5错误!未定义书签。航向姿态参考系的程序流程
5.6 手机端功能原理及Android代码
对手机端apk的修改主要是将两个独立的安卓apk进行了源码级的合并. 5.6.1 蓝牙帧处理
蓝牙模块方面，使用了开源的蓝牙串口通讯代码。在进行合并的过程中,主要对代码的接收模块进行了修改。由于蓝牙的发送和接收以及Android广播中传递的均为字符串，因此,为了解析方便，我们将蓝牙帧设置为如下格式：
u3_printf(”P:%c%.1f R：%c%.1f Y：%c％.1f\\n\，Pitch,cr,Roll，cy,Yaw）； 其中，cp、cr、cy分别为Pitch、Roll、Yaw的符号，为非负数时显示为空格符。这里注意到在进行格式化输出的时候，控制小数点后保留一位小数，但是小数点前的位数无法控制，猜测是由于c语言头文件功能较为精简造成的。
5.6.2 参数的广播传递
为了实现参数传递的功能，查阅了大量Android相关的代码和资料，最终选择了通过Android特有的广播机制进行参数的传递。此外，在广播的过程中，还是用了意图(Intent),这也是Android特有的一个机制，用来对广播进行定向生成和接收,保证三维制图端收到的数据为蓝牙接收到并进行广播的数据.
另外在对蓝牙数据进行接收时，发现无法理想地收取到单个帧数据，大部分时候是一次性获取到了几十帧的数据，其中第一帧和最后一帧往往是不完整的,这就要求我们对数据进行处理之后再进行广播:
//接收数据线程 while(true）{ num = is。read(buffer）; //读入数据 n=0； String s0 = new String（buffer，0,num)； fmsg+=s0; //保存收到数据 for（i=0；i 在合并过程中，由于涉及到视图的切换，因此对Android操作系统的进程管理和线程创建进行了学习，最终能够实现从蓝牙接收视图到三维绘制视图的平滑切换。切换的时机最终选择在蓝牙连接建立后，开始接收数据的瞬间: //打开接收线程 try｛ is = _socket.getInputStream()； //得到蓝牙数据输入流 }catch(IOException e){ Toast.makeText(this， \"接收数据失败！”, Toast.LENGTH_SHORT).show(); return; } if（bThread==false）｛ ReadThread.start（）； bThread=true； Intent intent = new Intent(BTClient.this， mainActivity。class）; intent.putExtra（”CC0”， smsg）; startActivity（intent）; }else{ bRun = true； ｝ 5.6.4 OpenGL绘图 三维绘图方面,使用了官方提供的示例代码，主要在每一帧绘制的过程中加 入了参数传递的功能，从而实现了实时从蓝牙接收数据并绘制三维图形： TestReceiver testReceiver; protected void init（GL10 gl) { gl。glClearColor(0.0f, 0。0f, 0.0f, 1.0f)； gl.glEnable（GL10。GL_DEPTH_TEST）； gl。glEnable（GL10.GL_CULL_FACE）； gl。glClearDepthf(1.0f); gl.glDepthFunc(GL10.GL_LEQUAL）； gl.glClearDepthf（1。0f） gl.glShadeModel(GL10。GL_SMOOTH); //实例化接收类 testReceiver = new TestReceiver（); } public void onDrawFrame（GL10 gl) ｛ // TODO Auto—generated method stub gl。glClear（GL10。GL_COLOR_BUFFER_BIT | GL10.GL_DEPTH_BUFFER_BIT）； //获取广播的数据 String allString = testReceiver.getString（）； //对广播数据进行解析，从字符串转化为float类型 if （allString。length()>=20) { Pitch=Float.parseFloat(allString.substring(allString。indexOf（”P：”)+2, allString。indexOf(\"R:\"）—1）); Roll=Float.parseFloat(allString。substring（allString。indexOf(\"R：”）+2, allString。indexOf（\"Y:”）—1)); Yaw=Float。parseFloat(allString。substring（allString。indexOf（”Y:”)+2， allString.length（）))； ｝ gl。glMatrixMode（GL10。GL_MODELVIEW); gl。glLoadIdentity()； GLU.gluLookAt(gl， 0， 0， 3, 0， 0， 0, 0, 1, 0）； gl.glVertexPointer（3， GL10。GL_FLOAT， 0， cubeBuff); gl.glEnableClientState（GL10.GL_VERTEX_ARRAY); gl.glRotatef（xrot, 1, 0， 0); gl.glRotatef（yrot, 0, 1， 0）; gl.glRotatef（zrot， 0, 0， 1); gl。glColor4f（1。0f, 0， 0, 1。0f); gl.glDrawArrays(GL10。GL_TRIANGLE_STRIP， 0， 4)； gl。glDrawArrays（GL10。GL_TRIANGLE_STRIP， 4, 4); gl。glColor4f(0， 1.0f， 0, 1.0f）; gl。glDrawArrays(GL10.GL_TRIANGLE_STRIP， 8, 4）; gl。glDrawArrays(GL10.GL_TRIANGLE_STRIP, 12， 4)； gl.glColor4f（0， 0, 1.0f， 1。0f）; gl。glDrawArrays（GL10.GL_TRIANGLE_STRIP, 16, 4); gl.glDrawArrays(GL10。GL_TRIANGLE_STRIP, 20， 4)； //将读取到的欧拉角赋值给三维图形的旋转角 xrot = Pitch; yrot = -Roll; zrot = Yaw; } 6. 系统测试 由于三维电子旋转台和加速度测量仪价格过于昂贵，因此，只是利用Matlab软件进行了简单的系统测试。 6.1 AHRS测试 我组对从串口获取的MPU6050数据进行了处理，获得到浮点型数据，并对使用AHRS和不使用AHRS进行了测试比较，由于数据量较大，这里我们用图像展示,结果如下： 图6.1错误!未定义书签。未进行AHRS处理得到的姿态变化 图6.1错误!未定义书签。AHRS处理后得到的姿态变化 由图可看出，不使用AHRS的数据绘出的图像与实际姿态完全不对应，误差较大。而使用了AHRS处理的数据较稳定，且比较符合实际姿态. 6.2 高通滤波算法 根据我组需要实现的还原MPU6050六轴传感器姿态变化的功能,我们需要对获取的MPU6050加速度传感器数据进行处理，通过二次积分得到MPU6050的姿态的位移。由于加速度传感器的输出存在固定的零点漂移，即当实际加速度为0时，传感器输出并不一定为0，而是一个非零输出。 若设传感器的输出值为a(t)+Aerror,其中a(t)为实际加速度，则在对传感器输出值的误差Aerror进行二次积分后会产生积分累计效应，导致误差更大。同时，若传感器初始化时加速度不为0,也会导致二次积分后产生积分累计效应。 通过查阅资料,高通滤波可以解决零点漂移问题，我组选择使用高通滤波处理数据，而Matlab提供了高通滤波函数filtfilt（)，我组调用函数处理的主要代码如下： ％％ High—pass filter linear velocity to remove drift order = 1; filtCutOff = 0.1; [b， a］ = butter（order， (2*filtCutOff）/(1/samplePeriod）, 'high’）； linVelHP = filtfilt（b, a， linVel); %linVelHP = linVel; ％ Plot figure（’Number', ’off’， ’Name’, 'High—pass filtered Linear Velocity’）； hold on； plot（linVelHP(:,1)， 'r'）; plot(linVelHP（:,2)， 'g’）; plot（linVelHP(：,3）, 'b'); xlabel（'sample')； ylabel（’g'）； title（'High—pass filtered linear velocity’)； legend（’X', ’Y’， ’Z’)； 其中samplePeriod为取样周期，取为1/256;buffer（)为分段函数，将数据分段进行滤波；plot（）为绘图函数。我们对使用高通滤波和不使用高通滤波进行了测试比较，由于数据量较大，我们以图像的形式展现，结果如下图： 图6.2错误!未定义书签。未进行高通滤波处理得到的速度变化 图6.2错误!未定义书签。高通滤波处理后得到的速度变化 如图 6。2-2可看出，不使用高通滤波处理得到的数据由于零点漂移导致的二次积分误差，趋于正向最大或负向最小；而使用高通滤波处理得到的数据趋于稳定,且动作被放大，变得更明显. 6.3 实时姿态检测系统测试 当时演示时，提出要将手机固定在板上进行测试，结果是偏差非常大，这与实际情况有很大差别。后来找到了原因,当时代码的编写是将数据作为正反馈传递给绘图程序的,而现在为了检验精度，需要改为负反馈，同时，为了便于观察，将3D图形替换为了一条直线，代码改变如下(原始代码已被注释)： //gl。glDrawArrays（GL10。GL_TRIANGLE_STRIP, 0， 4）; gl.glDrawArrays(GL10。GL_LINE_STRIP， 2, 2); xrot = —Pitch; yrot = Roll； zrot = -Yaw； /＊xrot = Pitch； yrot = —Roll; zrot = Yaw；＊/ 如图,为测试结果，可以看到误差逐渐变大,最后无法复位： 7. 关键代码说明 7.1 IIC关键代码 7.1.1 初始化IIC //初始化IIC void IIC_Init(void） { GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd（RCC_AHB1Periph_GPIOB， ENABLE);//使能GPIOB时钟 //GPIOB8,B9初始化设置 GPIO_InitStructure。GPIO_Pin = GPIO_Pin_8 ｜ GPIO_Pin_9； GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT；//普通输出模式 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP；//推挽输出 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz GPIO_InitStructure。GPIO_PuPd = GPIO_PuPd_UP；//上拉 GPIO_Init(GPIOB， ＆GPIO_InitStructure);//初始化 IIC_SCL=1； IIC_SDA=1； } 通过IIC_Init函数可看出，函数主要对GPIO的PB8、PB9引脚进行了初始化设置。先使能GPIOB时钟，然后对PB8、PB9引脚的相关寄存器进行初始化设置，设置完毕后通过GPIO_Init函数将初始化设置有效，最后将SCL和SDA使能。 7.1.2 数据处理 //IO方向设置 #define SDA_IN（) { GPIOB—〉MODER&=~（3<<(9＊2)）；GPIOB-〉MODER|=0〈〈9*2; //PB9输入模式 ｝ ＃define SDA_OUT() ｛ GPIOB-〉MODER＆=～（3<<（9*2）);GPIOB->MODER｜=1〈<9＊2；//PB9输出模式 ｝ 上述代码定义了SDA_IN和SDA_OUT两个函数,通过设置PB9引脚功能来设置IIC_SDA接口是输入模式还是输出模式。有了这两个函数之后，在IIC函数文件中，就能够使用IIC_Send_Byte和IIC_Read_Byte函数来发送或读取数据。具体代码如下： //IIC发送一个字节 //返回从机有无应答 //1，有应答 //0，无应答 void IIC_Send_Byte（u8 txd）｛ u8 t； SDA_OUT（); IIC_SCL=0;//拉低时钟开始数据传输 for(t=0；t〈8；t++）{ IIC_SDA=（txd＆0x80）〉〉7； txd〈<=1； delay_us（2)；//对TEA5767这三个延时都是必须的 IIC_SCL=1； delay_us（2)； IIC_SCL=0； delay_us（2）; } } //读1个字节，ack=1时，发送ACK，ack=0，发送nACK u8 IIC_Read_Byte（unsigned char ack）{ unsigned char i,receive=0; SDA_IN（)；//SDA设置为输入 for（i=0；i<8；i++ ){ IIC_SCL=0; delay_us(2）； IIC_SCL=1； receive<〈=1； if（READ_SDA)receive++； delay_us(1）; ｝ if (!ack) IIC_NAck（）；//发送nACK else IIC_Ack(）； //发送ACK return receive; } 7.2 MPU6050关键代码 7.2.1 初始化 根据5.2。3中说明的,对MPU6050初始化,需要先初始化IIC接口，然后复位MPU6050，接着设置相关参数，最后配置系统时钟源并使能加速度传感器和陀螺仪。具体代码如下： //初始化MPU6050 //返回值:0，成功 //其他,错误代码 u8 MPU_Init(void)｛ u8 res； IIC_Init();//初始化IIC总线 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80)； //复位MPU6050 delay_ms(100)； MPU_Write_Byte（MPU_PWR_MGMT1_REG,0X00)； //唤醒MPU6050 MPU_Set_Gyro_Fsr（0）；//陀螺仪传感器，3 ±2000dps MPU_Set_Accel_Fsr（0);//加速度传感器,±2g MPU_Set_Rate(50）;//设置采样率50Hz MPU_Write_Byte（MPU_INT_EN_REG，0X00)；//关闭所有中断 MPU_Write_Byte（MPU_USER_CTRL_REG,0X00)；//I2C主模式关闭 MPU_Write_Byte(MPU_FIFO_EN_REG，0X00）；//关闭FIFO MPU_Write_Byte（MPU_INTBP_CFG_REG，0X80);//INT引脚低电平有效 res=MPU_Read_Byte（MPU_DEVICE_ID_REG); if（res==MPU_ADDR)//器件ID正确 { } MPU_Write_Byte（MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考 MPU_Write_Byte(MPU_PWR_MGMT2_REG，0X00）; //加速度与陀螺仪都工作 MPU_Set_Rate（50);//设置采样率为50Hz }else return 1； return 0; 7.2.2 设置寄存器 在MPU6050初始化中，需要设置角速度传感器（陀螺仪）和加速度传感器的满量程范围以及采样率，而根据5。2.4需要通过修改寄存器来设置，所以用到MPU_Write_Byte函数来对寄存器进行写操作。具体代码如下： //设置MPU6050陀螺仪传感器满量程范围 //fsr:0，±250dps；1，±500dps；2,±1000dps；3，±2000dps //返回值:0，设置成功 //其他，设置失败 u8 MPU_Set_Gyro_Fsr(u8 fsr){ return MPU_Write_Byte（MPU_GYRO_CFG_REG,fsr〈<3)； //设置陀螺仪满量程范围 } //设置MPU6050加速度传感器满量程范围 //fsr:0，±2g;1,±4g；2，±8g;3，±16g //返回值:0，设置成功 //其他，设置失败 u8 MPU_Set_Accel_Fsr（u8 fsr）{ return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr〈<3)；//设置加速度传感器满量程范围 } //设置MPU6050的采样率(假定Fs=1KHz） //rate:4~1000（Hz) //返回值：0，设置成功 //其他，设置失败 u8 MPU_Set_Rate（u16 rate){ u8 data; } if（rate〉1000)rate=1000; if(rate<4）rate=4； data=1000/rate—1； data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG，data)； //设置数字低通滤波器 return MPU_Set_LPF（rate/2）;//自动设置LPF为采样率的一半 7.2.3 从数据输出寄存器读取数据 在MPU6050初始化完成后，需要通过加速度传感器数据输出寄存器和陀螺仪数据输出寄存器来读取加速度传感器数据和陀螺仪数据，所以用到MPU_Read_Len函数来对寄存器进行读操作。具体代码如下： //得到陀螺仪值(原始值) //gx,gy,gz：陀螺仪x,y，z轴的原始读数（带符号） //返回值：0，成功 //其他，错误代码 u8 MPU_Get_Gyroscope(short *gx,short ＊gy，short *gz){ u8 buf［6］,res； res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG，6,buf）; if(res==0) { *gx=（（u16）buf［0]<〈8)｜buf[1］; *gy=（（u16）buf［2］〈〈8)|buf［3］; ＊gz=（(u16）buf［4］〈<8)|buf［5]； ｝ return res；； ｝ //得到加速度值(原始值） //gx，gy,gz：陀螺仪x,y,z轴的原始读数（带符号) //返回值:0,成功 //其他,错误代码 u8 MPU_Get_Accelerometer（short *ax，short *ay，short *az）{ u8 buf[6],res; res=MPU_Read_Len（MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf); if（res==0) { *ax=（(u16)buf[0］<〈8)|buf[1]； ＊ay=（（u16）buf[2]〈〈8)|buf[3]； *az=（（u16）buf[4］〈〈8)｜buf[5］; } return res；； } 7.3 DMP关键代码 7.3.1 初始化 mpu_dmp_init(）是MPU6050 DMP初始化函数，该函数具体代码如下： //mpu6050,dmp初始化 //返回值:0,正常 //其他,失败 u8 mpu_dmp_init(void）{ u8 res=0; IIC_Init（)；//初始化IIC总线 if（mpu_init(）==0） ｛//初始化MPU6050 //设置需要的传感器 res=mpu_set_sensors（INV_XYZ_GYRO|INV_XYZ_ACCEL）; if（res)return 1; res=mpu_configure_fifo（INV_XYZ_GYRO｜INV_XYZ_ACCEL）;//设置FIFO if（res）return 2； res=mpu_set_sample_rate（DEFAULT_MPU_HZ）； //设置采样率 if（res)return 3； res=dmp_load_motion_driver_firmware（);//加载dmp固件 if（res）return 4; //设置陀螺仪方向 res=dmp_set_orientation(inv_orientation_matrix_to_scalar（gyro_orientation））； if(res)return 5； //设置dmp功能res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT｜DMP_FEATURE_TAP｜ DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL| DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL); if（res)return 6; //设置DMP输出速率(最大200Hz) res=dmp_set_fifo_rate(DEFAULT_MPU_HZ)； if(res)return 7; res=run_self_test(）； //自检 if(res）return 8； res=mpu_set_dmp_state（1); //使能DMP if（res)return 9； ｝ return 0； } 函数首先通过IIC_Init（需外部提供）初始化与MPU6050连接的IIC接口，然后调用mpu_init函数，初始化MPU6050,之后就是设置DMP所用传感器、FIFO、采样率和加载固件等一些列操作，在所有操作都正常之后，最后通过 mpu_set_dmp_state(1)使能DMP功能,在使能成功以后才可以通过mpu_dmp_get_data来读取姿态解算后的数据。 7.3.2 读取数据 mpu_dmp_get_data(）函数用于得到DMP姿态解算后的俯仰角、横滚角和航向角。具体代码如下: //得到dmp处理后的数据（注意，本函数需要比较多堆栈，局部变量有点多） //pitch：俯仰角精度：0.1°范围:—90。0° 〈——-〉 +90.0° //roll：横滚角精度：0。1°范围：-180.0°<--—〉 +180.0° //yaw：航向角精度：0。1°范围：-180。0°<---〉 +180.0° //返回值：0,正常 //其他，失败 u8 mpu_dmp_get_data（float ＊pitch,float ＊roll，float ＊yaw)｛ float q0=1。0f，q1=0.0f,q2=0。0f，q3=0.0f; unsigned long sensor_timestamp； short gyro［3]， accel[3］， sensors; unsigned char more; long quat[4］； if（dmp_read_fifo(gyro， accel, quat, &sensor_timestamp, &sensors,＆more）) return 1; if（sensors&INV_WXYZ_QUAT） { q0 = quat[0］ / q30； //q30格式转换为浮点数 q1 = quat［1] / q30； q2 = quat［2] / q30; q3 = quat[3］ / q30； //计算得到俯仰角/横滚角/航向角 *pitch = asin(-2 ＊ q1 * q3 + 2 ＊ q0＊ q2)* 57。3; ＊roll = atan2(2 ＊ q2 ＊ q3 + 2 ＊ q0 * q1, -2 ＊ q1 * q1 - 2 * q2* q2 + 1)＊ 57.3; ＊yaw= atan2(2*(q1＊q2 + q0*q3)，q0*q0+q1*q1—q2*q2-q3*q3） * 57。3; ｝else return 2； return 0; ｝ 7.4 AHRS关键代码 function obj = Update(obj， Gyroscope, Accelerometer, Magnetometer） q = obj。Quaternion; ％ short name local variable for readability ％ Normalise accelerometer measurement if(norm(Accelerometer) == 0）, return; end % handle NaN Accelerometer = Accelerometer / norm(Accelerometer)； ％ normalise magnitude % Normalise magnetometer measurement if（norm(Magnetometer) == 0）， return; end % handle NaN Magnetometer = Magnetometer / norm（Magnetometer)； ％ normalise magnitude ％ Reference direction of Earth’s magnetic feild h = quaternProd（q， quaternProd([0 Magnetometer]， quaternConj（q))); b = ［0 norm(［h(2) h(3）］) 0 h(4)]; % Estimated direction of gravity and magnetic field v = [2＊(q(2）＊q(4） — q（1）*q（3)） 2*(q(1)*q(2） + q（3）*q(4)） q（1）^2 — q（2)^2 - q（3）^2 + q(4)^2]; w = [2＊b（2）＊(0。5 - q（3)^2 — q(4)^2） + 2*b（4）*（q（2)*q(4） — q(1)*q(3)) 2＊b（2)＊(q(2）＊q（3） — q（1)＊q（4）) + 2＊b（4)＊(q(1)*q（2） + q（3)＊q（4)) 2*b（2）＊(q（1)*q（3） + q（2）＊q(4)） + 2＊b(4)*(0.5 - q（2）^2 — q（3）^2)］; % Error is sum of cross product between estimated direction % and measured direction of fields e = cross(Accelerometer, v） + cross（Magnetometer, w); if(obj。Ki 〉 0） obj.eInt = obj.eInt + e ＊ obj.SamplePeriod； else obj。eInt = [0 0 0]; end ％ Apply feedback terms Gyroscope = Gyroscope + obj。Kp * e + obj。Ki * obj.eInt； % Compute rate of change of quaternion qDot = 0。5 ＊ quaternProd(q, [0 Gyroscope（1) Gyroscope(2) Gyroscope(3）]）; ％ Integrate to yield quaternion q = q + qDot * obj.SamplePeriod; obj.Quaternion = q / norm（q)； % normalise quaternion end function obj = UpdateIMU(obj, Gyroscope， Accelerometer) q = obj。Quaternion； % short name local variable for readability ％ Normalise accelerometer measurement if(norm（Accelerometer) == 0）, return； end % handle NaN Accelerometer = Accelerometer / norm(Accelerometer); ％ normalise magnitude % Estimated direction of gravity and magnetic flux v = ［2＊（q(2)＊q（4) — q(1)＊q（3)) 2＊（q(1）*q（2） + q(3)＊q(4)） q(1）^2 — q（2)^2 - q(3）^2 + q（4）^2］； % Error is sum of cross product between estimated direction ％ and measured direction of field e = cross（Accelerometer, v）； if（obj.Ki 〉 0） obj.eInt = obj.eInt + e ＊ obj.SamplePeriod; else obj。eInt = [0 0 0]； end ％ Apply feedback terms Gyroscope = Gyroscope + obj。Kp * e + obj。Ki * obj。eInt； % Compute rate of change of quaternion qDot = 0.5 ＊ quaternProd（q, ［0 Gyroscope(1） Gyroscope(2） Gyroscope（3)］); % Integrate to yield quaternion q = q + qDot ＊ obj。SamplePeriod； obj.Quaternion = q / norm（q)； % normalise quaternion end 7.5 卡尔曼滤波 7.5.1 四维扩展卡尔曼滤波算法 扩展卡尔曼滤波算法(Extended Kalman Filter, EKF）是一套由计算机实现的实时递推算法，所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要求的估计值(系统的状态变量）作为滤波器的输出,滤波器的输入和输出由时间更新和观测更新算法联系在一起，根据系统的状态方程和观测方程估算出所需要处理的信号。AHRS扩展卡尔曼滤波算法的状态变量采用四维四元数，与采用欧拉角相比，避免了采用欧拉角计算时涉及的大量三角函数运算,保证了更新速率和实时性，同时不存在采用欧拉角运算出现的奇异性。欧拉角与四元数的转换关系如下： ····················（1） ··························(2） ··························（3) 四元数微分方程如式（4）所示: ············(4） 四元数姿态矩阵微分方程只要解4个微分方程，比方向余弦姿态矩阵微分方 程减少了大量的运算，便于微处理器的编程实现。 7.5.2 时间更新 系统的状态方程如式（5)所示。 ·······································(5) 其中状态变量为四元数 矩阵A可以根据陀螺仪测得的三轴角速率示。其中△t为两次时间预测更新所流逝的时间。 ， 为四维过程噪声。得到，如式（6）所 ·····(6） 状态变量的时间更新如式（7）所示. ·····················································（7) 协方差矩阵 预测如式（8）所示，式中Q为四维过程激励噪声协方差. ········································（8） 7.5.3 观测更新 AHRS的观测更新是通过本体坐标系上的重力加速度和地磁场的参考矢量旋转至导航坐标系上，再与加速度和磁场传感器比较,得到观测变量的残余.由本体系转换至导航系的转移矩阵由四元数可以表示为式（9）。 ··（9） 三位参考向量v转移至导航系中可有观测方程式(10)表示 ···································(10) 当重力加速度观测更新时参考向量v等于重力加速度参考矢量(可设置为当平台静止水平放置时，加速度计测量得到的三维矢量为： 当磁场观测更新时v等于磁场参考矢量(可设置为当平台静止水平放置且航向指向正北时，磁阻计测量得到的三维矢量为： H是h对X求偏导的雅可比矩阵，如式(11）所示。 （11） 卡尔曼增益矩阵 如式(12)所示，式中R阵为三维观测噪声协方差矩阵： ······················（12） 观测更新： ··························(13) 当重力加速度观测更新时为加速度，传感器测量得到的三维矢量: 当磁场观测更新时为磁阻传感器，测量得到的三维矢量： 协方差更新： ····································· (14） 7.6 ATK-HC05关键代码 7.6.1 初始化ATK-HC05模块 //初始化ATK—HC05模块 //返回值：0，成功;1,失败. u8 HC05_Init（void) { u8 retry=10，t; u8 temp=1; GPIO_InitTypeDef GPIO_InitStructure； RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF ｜RCC_AHB1Periph_GPIOC, ENABLE）;//使能GPIOC,GPIOF时钟 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; //LED对应引脚 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN；//普通输入模式 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M GPIO_InitStructure。GPIO_PuPd = GPIO_PuPd_UP;//上拉 GPIO_Init（GPIOC， &GPIO_InitStructure）;//初始化GPIOC0 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6；//KEY对应引脚 GPIO_InitStructure。GPIO_Mode = GPIO_Mode_OUT；//普通输出模式 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP；//推挽输出 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz；//100MHz GPIO_InitStructure。GPIO_PuPd = GPIO_PuPd_UP;//上拉 GPIO_Init（GPIOF, &GPIO_InitStructure); //根据设定参数初始化PF6 GPIO_SetBits(GPIOF,GPIO_Pin_6)； ｝ usart3_init(9600); //初始化串口3为：9600，波特率。 while(retry--) { HC05_KEY=1; //KEY置高，进入AT模式 delay_ms(10）； u3_printf（”AT\\r\\n”)；//发送AT测试指令 HC05_KEY=0； //KEY拉低,退出AT模式 for(t=0;t<10；t++) {//最长等待50ms，来接收HC05模块的回应 if（USART3_RX_STA&0X8000)break; delay_ms（5）; } if（USART3_RX_STA&0X8000）｛//接收到一次数据了 temp=USART3_RX_STA＆0X7FFF;//得到数据长度 USART3_RX_STA=0； if（temp==4&&USART3_RX_BUF[0]=='O'＆&USART3_RX_BUF［1］=='K’）｛ temp=0；//接收到OK响应 break; ｝ } ｝ if(retry==0)temp=1; //检测失败 return temp； 7.6.2 状态显示 //显示ATK-HC05模块的主从状态 void HC05_Role_Show(void） { if(HC05_Get_Role()==1)LCD_ShowString（240，180，200，16,16,”ROLE:Master\"）; //主机 else LCD_ShowString(240，180，200，16，16,\"ROLE：Slave ”）； //从机 } //显示ATK—HC05模块的连接状态 void HC05_Sta_Show（void） ｛ if（HC05_LED）LCD_ShowString（330,180,120，16，16,\"STA:Connected \"）;//连接成功 else LCD_ShowString(330,180，120,16，16，\"STA：Disconnect\");//未连接 } 8. 遇到的问题及解决方案 8.1 问题一 Matlab串口读数据 问题描述： 在开发板上调用了USART_SendData(）函数来通过串口输出数据。而我组编写代码时，按照函数传递的帧格式对数据帧进行了定义。每一帧为32个u8型数据组成的数组，其中第1个u8数据为帧头0x88,第2个为功能字，第3个为数据长度，从第4个到第31个为传递的数据，最后一个为校验和。 在Matlab中，我组通过set(）来设置串口参数，然后调用函数serial(）来获取串口信息，并通过fread(）函数来读取串口输出数据并进行进一步的处理. 而在处理数据过程中，我组发现在数据格式处理上，Matlab与开发板不相适应。首先是开发板的数据帧中加速度传感器和陀螺仪的十六位数据都按照USART_SendData（)函数的要求被分为了高八位和低八位，并存在不同的u8型数组中，在使用Matlab获取到串口的数据后,需要对这些被拆分的数据进行整合以获取我们需要的数据，但是Matlab的位处理不适宜处理这些u8型数据。其次是开发板输出的数据为u8类型的数据,而通过Matlab函数读取出串口数据后，Matlab会自动将数据处理为double型并存储在数组中。当我们将数据强制转换为u8型或int8型或uint16型或int16型时，溢出的数据Matlab会将其溢出部分舍弃，并取最大值或最小值，从而导致数据错误，影响到后面的计算。 解决方案： 在经过许多次的数据读取、校验等操作后,我组采用了乘积德方法解决低八位与高八位数据整合的问题，即加速度传感器/陀螺仪数据=高八位u8数据＊2^8+低八位u8数据.然而由于Matlab将读取的数据处理成double型的问题，在应用了上述的公式后会出现溢出的情况,即Matlab将溢出的数据均取为最小值（下溢）或最大值（上溢）.在尝试多种方式后，我组采用比较简单的处理方式即判断下溢或上溢的情况，若上溢则将结果减去最大值，取溢出部分；若下溢则将结果加上最大值，取出溢出的正数部分。取溢出部分的原因是，根据十六进制的四则运算规则以及数据结构特征,当两个数相加发生溢出时,溢出的最高位会被舍去，而剩下的其实就是溢出的部分。 8.2 问题二蓝牙数据帧的发送和接收 问题描述： 手机端蓝牙接收到的数据不是想象中条理分明的样子,而是很多条数据连在一起，并且部分数据还存在残缺的现象，无法直接进行广播与参数提取。 广播携带的数据类型为字符串，需要将其转化为浮点型数据才能进行参数的正确使用。 解决方案： 在广播发送之前,需要对大量的蓝牙数据帧进行分离，然后分别进行广播： String tmp[］=toSend.split（”\\n”）； if（toSend.length()>=21）{ for（int len=0;len Pitch=Float.parseFloat（allString。substring(allString.indexOf(\"P:”)+2， allString。indexOf(”R:”)-1）); Roll=Float.parseFloat(allString。substring(allString。indexOf(”R:\")+2， allString。indexOf(\"Y：”)—1)）； Yaw=Float。parseFloat（allString.substring(allString。indexOf（\"Y:\")+2, allString。length()）); 8.3 问题三Android参数传递 问题描述： Android由众多Activity组成，每个Activity对应不同的功能和UI,但是每个Activity都是一个单独的类,所以需要传递参数。 由于OpenGL绘图时，每一帧都要对xrot、yrot、zrot的值进行查询，因此需要不断发送接收到的蓝牙数据帧，从而将参数顺利传递到onDrawFrame（）函数进行图形绘制。 由于蓝牙发送速率的限制，导致onDrawFrame（)函数接收到的参数在很时间内不会发生改变. 解决方案： 这里选择通过Intent（意图)进行广播. Intent是Android的类,含有传递参数的方法. 在建立广播时，注意需要在AndroidMainfest。xml文件中注册接收器，这里设置的广播ID为“CCSendflag\": 〈receiver android:name=”.TestReceiver”〉 到目前为止，我组已实现了在PC端使用Matlab展示开发板上MPU6050的姿态变化，以及在手机端展示陀螺仪的姿态变化。在PC端能够使用Matlab大致还原出MPU6050六轴传感器的姿态变化，其中通过加速度传感器数据还原其位移变化，用陀螺仪数据还原其方向变化.在手机端能够在可接受的时间延迟内实时的展现MPU6050陀螺仪的姿态变化，能够通过DMP算法得到的三个欧拉角在手机端绘出立方体，代替陀螺仪的姿态. 目前仍存在两个主要问题：一个问题是在PC端用Matlab处理得到的数据仍存在一定的误差，还原出的姿态变化仍不够准确，而且有一定几率出现错乱的情况，即还原的姿态与实际姿态不对应；另一个问题是在手机端展示时仍存在一定的时间延迟，且与实际姿态有一定的误差，当手机中的立方体与开发板处于同一初始角度时，手机上的立方体展现的角度与实际角度略有偏差. 针对上述问题，我组计划在接下来主要实现两个工作:一个是对PC端处理MPU6050六轴传感器的数据的算法进行进一步的测试和优化，将还原出的姿态改进得更接近于实际姿态；另一个是对手机端数据处理进行优化，进一步减小时间延迟，做到展示的立方体姿态与MPU6050的同步,同时改进算法，减小立方体展现的角度存在的偏差. 10. 心得体会 我组最初的项目目标是利用MPU6050实现一个小球走迷宫的游戏.在大致熟悉了开发板以及MPU6050工作原理后，我们通过调用封装好的MPU6050获取数据函数以及LCD屏绘图函数初步实现了通过MPU6050控制小球运动轨迹的功能，小球能够朝着传感器倾斜的方向运动，并能够根据倾斜的角度来调整运动速度。在实现该功能后，我们简单的了解了开发板UCGUI界面编程，发现在STM32F4探索者开发板上编写3D图形较为困难，所以我们放弃了实现小球走迷宫的游戏的目标.而后，经过广泛查阅资料,我们找到一个新的目标，即惯性导航。 惯性导航即利用惯性元件来测量运载体本身的加速度，经过积分和运算得到速度和位置，从而达到对运载体导航定位的目的。我们将项目目标更改为能够将MPU6050的数据传输到手机或电脑上，在手机端或PC端处理数据并进行进一步的编程开发.然而手机端需要通过蓝牙传输数据，而PC端可以通过串口获取数据。我们想在手机端处理数据并绘制3D图像来展现MPU6050的姿态变化，在PC端能够通过算法积分计算出位移和相对位置并进行校正，在一定程度上还原出MPU6050的姿态和位移变化，实现一个简单的惯性导航。 在开发前我们花费大量时间去阅读理解MPU6050的实现函数以及一些配置函数和其它相关部件的操作函数。在开发过程中，我们一遍又一遍的编写调试代码，并根据测试数据查找问题，讨论解决问题,优化问题.我们在网上查阅了大量 资料，阅读参考网上相关算法的实现方式，将不同的算法的实现方式的优点融合到一起,初步优化算法，并一遍又一遍地模拟,一遍又一遍地改进，最后在可接受的误差范围内初步实现了我们预期的功能。但是项目仍具有很大的改进需要,比如手机端的展示不够实时，PC端的还原不够对应，误差仍有缩小的改进空间。 在这个项目上,我们投入的时间和精力相对于其他课程项目更多一些，在开发过程中也遇到了许多不曾遇到过的困难，我们也尽可能的努力克服了，但仍有很多困难没有完全解决，仍需要进一步的研究。在之前我们也没有这样深入的研究过嵌入式，而我们拿到的开发板也是比较集成的，函数都是封装好的，可以直接调用，所以我们也没有过多的关注底层代码，对硬件层面的操作不够熟悉，也导致了在GPIO和中断上理解得不够透彻，在教员的指点下,我们花费了大量时间去研读底层代码，较之前更熟悉了GPIO和中断操作，对我们最后的功能实现也提供了较大的帮助. 整个开发过程中，学习是痛苦的，也是快乐的.痛苦是因为有太多困难需要解决、有太多代码需要理解；快乐是因为每解决一个困难便给自己增添了一份信心，每实现一个功能便意味着项目向前迈进了一步。在一段时间的不懈努力下,我们也取得了回报，即实现了我们预期的功能,虽然不够精准不够完美，但是我们从中学到了很多,为以后的学习开发打下了基础. 11. 文档版本更新说明 此文档为最终提交版本。 因篇幅问题不能全部显示，请点此查看更多更全内容