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

基于MPU6050的INS惯性导航和实时姿态检测系统
1. 项目目标及功能说明
1.1 项目目标
学习使用正点原子探索者开发板,并熟悉开发板上的MPU6050六轴传感器的工作原理和各函数的调用过程。同时学习开发板的扩展接口，尝试在开发板上扩展蓝牙模块，并实现开发板与手机等含有蓝牙模块的电子设备通过蓝牙连接并进行数据的传输.在完成上述内容的基础上，实现将MPU6050六轴传感器的加速度计和陀螺仪的数据传送到手机上，在手机上实现陀螺仪的变化效果展示。同时通过串口将MPU6050数据传送到电脑上，通过Matlab编程处理数据，实现惯性导航的简单展示。
1.2 系统功能说明
系统最主要的功能有两个:一个是在手机端能够展示开发板上MPU6050陀螺仪的姿态变化,通过一个立方体的转动来表示陀螺仪的转动；另一个是在电脑端能够读取MPU6050的数据，并通过对数据的处理还原数据中存储的MPU6050的姿态变化,简单展现出惯性导航的效果。
在实现系统最主要的两个功能过程中,还需要实现一些基础功能.开发板能够通过蓝牙与手机连接并传输数据；开发板能够通过串口将数据发送出去；在电脑端能够读取开发板上串口输出的数据等。
2. 需求分析
l 惯性导航系统用于各种运动机具中,包括飞机、潜艇、航天飞机等运输工具及导弹，然而成本及复杂性限制了其可以应用的场合.但是,存在一种情形：
卫星一旦突然因故障、敌方打击或干扰（如太阳风暴）等原因无法提供服务,这对依赖GPS、北斗等卫星导航系统作为唯一PNT(Position、Navigation、Time）信息来源的系统来说可能是致命的灾难。
作为目前为止卫星导航系统最好的备援-—惯性导航系统（INS）,将于届时发挥出巨大的作用，其精度完全可以媲美GPS等卫星导航系统。并且它不需要外部参考就可确定当前位置、方向及速度，从而使它自然地不受外界的干扰和欺骗.
定位、导航和授时服务对军队而言就像氧气对人类一样不可或缺，因此通过研究新机理、研制新设备、开发新算法，以摆脱人员和系统设备对GPS的依赖，具有极大的战略意义。
l 姿态监测系统可广泛应用于关键资产姿态变化的无线实时监控.由于目前移动智能终端设备的数量和质量逐步提升,因此，通过计算机上传统的上位机软件进行姿态监测，逐渐暴露出了自身的缺点-—串口传输无法实现无线监测、计算机相比智能终端便携性极差。
因此，使用无线传输（蓝牙、红外、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总线工作原理
1.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.1Error! Bookmark not defined.IIC总线时序图
探索者STM32F4开发板板载的EEPROM芯片型号为24C02。该芯片的总容量为256字节，通过IIC总线与外部连接。STM32F4开发板有硬件IIC，但是设计的比较复杂，而且稳定性不好，所以我组使用GPIO软件模拟IIC来对24C02进行读写.同时使用软件更具有移植性，只要有IO口，将软件移植过去就能使用模拟的IIC，而硬件必须MCU的支持.
1.1.2 硬件设计
实现模拟IIC需要用到的硬件资源有:串口（USMART）、GPIO、24C02。
图5.11STM32F4与24C02连接图
我组通过GPIO来模拟IIC，24C2的SCL和SDA分别连在GPIO_PB8和GPIO_PB9上，连接关系如图 5.1.2-1.
5.2 MPU6050工作原理
1.1.3 MPU6050引脚
图5.21MPU6050结构图
模块外观如图 5。2。1-2所示：
图5.2Error! Bookmark not defined.MPU6050实物图
图5.2Error! Bookmark not defined.MPU6050内部逻辑框图
如图 5.2。1-1为MPU6050六轴传感器的结构图，总共有24个引脚，而图 5。2。1-2为MPU6050的内部逻辑框图，描述了MPU6050内部的模块结构，以及各引脚的连接情况。
表1.1.3Error! Bookmark not defined.MPU6050引脚输出和信号描述
Pin Number
MPU—6050
Pin Name
Pin Description
1
Y
CLKIN
Optional external referenceclock input.Connect to GND if unused。
6
Y
AUX_DA
I2Cmasterserialdata， forconnecting to external sensors
7
Y
AUX_CL
I2CMasterserialclock， forconnecting to external sensors
8

/CS
SPI chipselect （0=SPI mode)
8
Y
VLOGIC
Digital I/O supplyvoltage
9

AD0 / SDO
I2CSlaveAddress LSB （AD0）； SPI serialdata output (SDO)
9
Y
AD0
I2CSlaveAddress LSB （AD0)
10
Y
REGOUT
Regulatorfiltercapacitorconnection
11
Y
FSYNC
Frame synchronizationdigital input。 Connectto GND ifunused.
12
Y
INT
Interruptdigital output （totempoleoropen—drain）
13
Y
VDD
Power supplyvoltageandDigital I/Osupply voltage
18
Y
GND
Power supplyground
19，21
Y
RESV
Reserved。Donot connect.
20
Y
CPOUT
Charge pump capacitorconnection
22
Y
RESV
Reserved.Donot connect。
23

SCL /SCLK
I2Cserialclock (SCL); SPI serialclock （SCLK)
23
Y
SCL
I2Cserialclock （SCL）
24

SDA / SDI
I2Cserialdata（SDA)； SPIserial data input （SDI)
24
Y
SDA
I2Cserialdata(SDA)
2，3， 4, 5, 14，
15，16， 17
Y
NC
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（不含最低位)。
1.1.4 硬件设计
图5.22MPU6050与STM32F4的连接电路图
从图 5.2.2—1可以看出，MPU6050通过三根线与STM32F4开发板连接,其中IIC总线时和24C02以及WM8978共用，接在PB8和PB9上面。MPU6050的中断输出，连接在STM32F4的PC0脚，不过本例程我们并没有用到中断.另外，AD0接的GND，所以MPU6050的器件地址是：0X68。
1.1.5 初始化操作
在使用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即可开启。
1.1.6 相关寄存器
在读取MPU6050数据中主要用到了以下寄存器:
(1) Power Management 1（电源管理寄存器1)
表1.1.61电源管理寄存器1各位描述
Register （Hex)
Register (Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
6B
107
DEVICE_RESET
SLEEP
CYCLE
-
TEMP_DIS

CLKSEL［2:0]

如表 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即可。
表1.1.62CLKSEL选择列表
CLKSEL[2:0］
时钟源
000
内部8M RC晶振
001
PLL,使用X轴陀螺作为参考
010
PLL,使用Y轴陀螺作为参考
011
PLL，使用Z轴陀螺作为参考
100
PLL，使用外部32。768Khz作为参考
101
PLL,使用外部19。2Mhz作为参考
110
保留
111
关闭时钟，保持时序产生电路复位状态
(2) Gyroscope Configuration(陀螺仪配置寄存器）
表1.1.63表陀螺仪配置寄存器各位描述
Register
（Hex）
Register(Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
1B
27
XG_ST
YG_ST
ZG_ST
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(加速度传感器配置寄存器）
表1.1.6Error! Bookmark not defined.加速度传感器配置寄存器各位描述
Register
(Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
1C
28
XA_ST
YA_ST
ZA_ST
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使能寄存器）
表1.1.6Error! Bookmark not defined.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(采样率分频寄存器)
表1.1.6Error! Bookmark not defined.陀螺仪采样率分频寄存器各位描述
Register
(Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
19
25

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(配置寄存器)
表1.1.6Error! Bookmark not defined.配置寄存器各位描述
Register
（Hex）
Register
（Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
1A
26
-
-

EXT_SYNC_SET［2：0]
DLPF_CFG［2：0］

如表5。2。4-7,寄存器地址为0x1A。加速度计和陀螺仪是根据数字低通滤波器（DLPF）的三个设置位进行过滤的，即DLPF_CFG[2：0］。DLPF_CFG不同配置对应的过滤情况如表 5。2。4-8所示：
表1.1.64DLPF_CFG配置表
DLPF_CFG［2：0]
加速度传感器
Fs=1Khz
角速度传感器（陀螺仪）
带宽(Hz）
延迟（ms)
带宽（Hz)
延迟（ms)
Fs（Khz）
000
260
0
256
0.98
8
001
184
2.0
188
1。9
1
010
94
3。0
98
2。8
1
011
44
4。9
42
4.8
1
100
21
8。5
20
8。3
1
101
10
13.8
10
13.4
1
110
5
19.0
5
18.6
1
111
保留
保留
8
加速度传感器输出速率（Fs）固定是1Khz,而角速度传感器的输出速率（Fs），则根据DLPF_CFG的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半,如前面所说的，如果设置采样率为50Hz，那么带宽就应该设置为25Hz，取近似值20Hz，就应该设置DLPF_CFG为100。
(7) Power Management 2（电源管理寄存器2）
表1.1.65电源管理寄存器2各位描述
Register
（Hex)
Register
（Decimal）
Bit7
Bit6
Bit 5
Bit 4
Bit 3
Bit2
Bit 1
Bit 0
6C
108
LP_WAKE_CTRL[1：0]
STBY
_XA
STBY
_YA
STBY
_ ZA
STBY
_ XG
STBY
_YG
STBY
_ZG
如表5。2.4-9,寄存器地址为0x6C。该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率，项目中没有用到。剩下的6位,分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式，由于不需要进入待机模式，所以全部设置为0.
(8) Gyroscope Measurements（陀螺仪数据输出寄存器)
表1.1.6Error! Bookmark not defined.陀螺仪数据输出寄存器各位描述
Register（Hex)
Register
(Decimal）
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
43
67
GYRO_XOUT［15：8］
44
68
GYRO_XOUT［7：0］
45
69
GYRO_YOUT［15：8］
46
70
GYRO_YOUT[7：0］
47
71
GYRO_ZOUT[15:8］
48
72
GYRO_ZOUT［7：0］
如表5。2。4—10,总共有6个寄存器,地址为0x43～0x48，通过读取这6个寄存器，就可以读到陀螺仪x/y/z轴的值，比如x轴的数据,可以通过读取0x43（高8位)和0x44(低8位）寄存器得到，其他轴以此类推.
(9) Accelerometer Measurements（加速度传感器数据输出寄存器）
表1.1.6Error! Bookmark not defined.加速度传感器数据输出寄存器各位描述
Register
（Hex）
Register
（Decimal)
Bit7
Bit6
Bit5
Bit4
Bit3
Bit2
Bit1
Bit0
3B
59
ACCEL_XOUT[15：8］
3C
60
ACCEL_XOUT［7：0]
3D
61
ACCEL_YOUT［15:8］
3E
62
ACCEL_YOUT［7：0]
3F
63
ACCEL_ZOUT［15:8］
40
64
ACCEL_ZOUT［7：0]
如表5。2。4—11，总共有6个寄存器，地址为0x3B～0x40，通过读取这8个寄存器，就可以读到加速度传感器x/y/z轴的值，比如读x轴的数据,可以通过读取0x3B(高8位)和0x3C（低8位）寄存器得到,其他轴以此类推.
5.3 ATK—HC05工作原理
1.1.7 ATK-HC05引脚
ATK—HC05 模块非常小巧（16mm*32mm），模块通过6个2。54mm间距的排针与外部连接,模块外观如图 5.3。1—1所示：
图5.31ATK—HC05 模块外观图
图 5。3.1—1中,从右到左，依次为模块引出的 PIN1～PIN6 脚,各引脚的详细描述如表5.3。1-1所示：
表1.1.71ATK—HC05模块各引脚功能描述
序号
名称
说明
1
LED
配对状态输出；配对成功输出高电平，未配对则输出低电平。
2
KEY
用于进入 AT 状态；高电平有效(悬空默认为低电平)。
3
RXD
模块串口接收脚（TTL 电平,不能直接接 RS232 电平!），可接单片机的 TXD
4
TXD
模块串口发送脚(TTL 电平,不能直接接 RS232 电平！），可接单片机的 RXD
5
GND
地
6
VCC
电源（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 秒闪一次）。
1.1.8 硬件设计
图5.3Error! Bookmark not defined. ATK-HC05 蓝牙串口模块原理图
模块与单片机连接最少只需要4根线即可：VCC、GND、TXD、RXD、VCC和GND用于给模块供电，模块TXD和RXD则连接单片机的RXD和TXD即可.该模块兼容5V和3。3V单片机系统，所以可以很方便的连接到系统里面去。
ATK—HC05模块与单片机系统的典型连接方式如图5。3.2-2所示：
图5.3Error! Bookmark not defined.ATK-HC05模块与单片机系统连接示意图
图中虚线连接表示可有可无,可以根据需要，选择性的使用。
1.1.9 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 所示：
表1.1.9Error! Bookmark not defined. ATK-HC05蓝牙模块同探索者STM32F407开发板连接关系表
ATK—HC05 蓝牙模块与开发板连接关系
ATK—HC05 蓝牙串口模块
VCC
GND
TXD
RXD
KEY
LED
探索者 STM32F407 开发板
5V
GND
PB11
PB10
PF6
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.3Error! Bookmark not defined. BTClient。apk运行界面
进入搜索蓝牙设备界面，如图 5。3。3-4所示:
图5.34搜索蓝牙设备
从上图可以看出，手机已经搜索到蓝牙模块了,HC—05,点击这个设备，输入默认密钥1234（仅第一次连接需要设置)，完成配对,如图 5。3。3—5所示:
图5.35输入配对密码
在输入密码之后,等待一段时间,即可连接成功,如图5。3.3—6所示：
图5.36连接成功
此时，手机和蓝牙模块就连接上了,手机便可以接收数据并绘制三维图形了，改变开发板的姿态,手机端绘制的三维图形如图 5。3。3—7所示:
图5.37手机绘制三维图形界面
这样,我们就实现了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
1.1.10 AHRS简介
AHRS称为航姿参考系统,包括多个轴向传感器，能够为飞行器提供航向,横滚和侧翻信息，这类系统用来为飞行器提供准确可靠的姿态与航行信息.
图5.51航姿参考系统
航姿参考系统包括基于MEMS的陀螺仪，加速度计和磁强计。航姿参考系统与惯性测量单元IMU的区别在于，航姿参考系统(AHRS)包含了嵌入式的姿态数据解算单元与航向信息，惯性测量单元（IMU)仅仅提供传感器数据，并不具有提供准确可靠的姿态数据.目前常用的航姿参考系统(AHRS)内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。
航姿参考系统(AHRS）具有两个主要特点:
(1) 高精度360 度全方位位置姿态输出，但采用欧拉角的会具有万向锁,不能全向转动.
(2) 高效的数据融合算法快速动态响应与长时间稳定性（无漂移，无积累误差）相结合。
航姿参考系统（AHRS)输出模式为三维全姿态数据（四元数/欧拉角/旋转矩阵).
1.1.11 AHRS软件设计
AHRS的软件设计主要分为：
① 传感器初始化，包括设置传感器的更新速率、量程.
② 初始化卡尔曼滤波的相关矩阵，根据传感器的特点设置过程激励噪声协方差矩阵Q，设为对角元素为0。1的四维对角方阵。
③ 若成功读取陀螺仪数据,进行卡尔曼滤波的时间更新。
④ 采集加速度传感器和磁阻传感器的数据，若读取成功则进行观测更新。加速度观测更新与磁场观测更新算法差别在于观测方差的R，可根据两种传感器的置信度没置相应的值,航向姿态参考系的程序流程如图 5。5。2—1所示。
图5.5Error! Bookmark not defined.航向姿态参考系的程序流程
5.6 手机端功能原理及Android代码
对手机端apk的修改主要是将两个独立的安卓apk进行了源码级的合并。
1.1.12 蓝牙帧处理
蓝牙模块方面，使用了开源的蓝牙串口通讯代码.在进行合并的过程中，主要对代码的接收模块进行了修改。由于蓝牙的发送和接收以及Android广播中传递的均为字符串，因此，为了解析方便,我们将蓝牙帧设置为如下格式：
u3_printf（”P：%c%.1f R：％c%.1f Y:%c％。1f\n”，cp,Pitch，cr，Roll，cy，Yaw);
其中,cp、cr、cy分别为Pitch、Roll、Yaw的符号,为非负数时显示为空格符。这里注意到在进行格式化输出的时候，控制小数点后保留一位小数,但是小数点前的位数无法控制，猜测是由于c语言头文件功能较为精简造成的。
1.1.13 参数的广播传递
为了实现参数传递的功能，查阅了大量Android相关的代码和资料，最终选择了通过Android特有的广播机制进行参数的传递。此外，在广播的过程中，还是用了意图（Intent)，这也是Android特有的一个机制，用来对广播进行定向生成和接收，保证三维制图端收到的数据为蓝牙接收到并进行广播的数据。
另外在对蓝牙数据进行接收时，发现无法理想地收取到单个帧数据，大部分时候是一次性获取到了几十帧的数据,其中第一帧和最后一帧往往是不完整的，这就要求我们对数据进行处理之后再进行广播：
//接收数据线程
while（true）｛
num = is.read（buffer）； //读入数据
n=0；
Strin