MPU6050传感器解析实验
19.1 mpu6050简介19.1.1 芯片概述mpu6050是invensense公司推出的一款6轴运动处理芯片，内置3轴陀螺仪及3轴速度传感器，内置两组i2c接口，其中一组用于通信，另一组则用于连接外部磁力传感器，采用自带的数字运动处理器dmp（digital motion processor），通过主i2c接口，直接读取完整的9轴融合演算数据。mpu6050检测轴及其检测方向如下图所示。
19.1.2 引脚介绍
mpu6050采用qfn-24封装，端口描述如下表所示。
引脚编号引脚名称功能
1 clkin 外部参考时钟输入，如果不使用直接接地
2 nc 空引脚
3 nc 空引脚
4 nc 空引脚
5 nc 空引脚
6 aux_da 从i2c接口数据口，用于连接磁传感器的sda组成九轴传感器
7 aux_cl 从i2c接口时钟口，用于连接磁传感器的scl组成九轴传感器
8 vlogic io口逻辑电平，最低可以设置1.8v，默认连接vdd
9 ad0 i2c接口地址控制端，端口为高电平默认地址0x69，端口为低电平默认地址0x68
10 regout 外接稳压器的滤波电容
11 fsync 帧同步数字输入，如果不使用直接接gnd
12 int 中断信号输出（可以配置为开漏输出）
13 vdd 电源正极，供电范围0.5v~6vdc
14 nc 空引脚
15 nc 空引脚
16 nc 空引脚
17 nc 空引脚
18 gnd 电源地
19 resv 保留
20 cpout 外部电荷泵电容
21 resv 保留
22 resv 保留
23 scl 主i2c接口时钟
24 sda 主i2c接口数据
19.1.3 硬件电路
由于mpu6050内部是可以自动计算x，y和z轴的方向及加速度的，使用者可以不考虑实际的数据转换问题，但是为了详细的了解mpu6050的计算过程，使用者最好还是应该具备了解原始数据如何转换为我们需要的角度与加速度值。
19.2 姿态解算与融合算法基础概念19.2.1 方向矩阵设有一个三位直角坐标系oxyz，如下图所示。
19.2.2 方向余弦矩阵
19.2.3 欧拉角欧拉角是用于确定定点转动缸体位置的3个1组的独立角参量，由章动角θ，旋转角（进动角）ψ和自转角φ组成，欧拉角有多种取法，下面是比较常见的一种。
如上图所示，由定点o做出固定坐标系oxyz以及固定连在刚体的坐标系ox’y’z’，以轴oz和oz’为基本轴。其垂直面oxy和ox’y’为基本平面，由轴oz量到oz’的角度θ称为章动角，平面zoz’的垂线on称为节线，同时on又是基本平面ox’y’和oxy的交线，在右手坐标系中，由on的正端看，角θ应按照逆时针方向计算，由固定轴ox到节线on的角度ψ称为进动角，也叫作旋转角，由节线on到动轴ox’的角度φ称为自转角，有oz和oz’正端看，进动角ψ与自转角φ也应该按照逆时针方向计算。
从上面的描述过程可以发现，欧拉角实际是可以分解成三步来计算的：
第1步：绕z轴旋转α，使得x轴与n轴重合
第2步：绕x轴旋转β，使z轴与旋转后的z轴重合
第3步：绕z轴旋转y，是坐标系与旋转后的完全重合
根据上面的三个步骤，我们来通过以下实例来说明欧拉角与方向余弦矩阵的转换过程。
19.2.4 四元数与欧拉角的转换四元数是一个简单的超复数，是由实数加上三个虚数单位i，j，k组成，每个四元数都是1，i，j，k的线性组合，四元数是爱尔兰数学家哈密顿在1843年发明的数学概念，四元数的乘法不符合交换律。
四元数姿态表达式是一个四参数的表达式，它的基本思路是一个坐标系转换到另一个坐标系可以通过绕一个定义在参考系中的矢量μ的单次转动来实现，四元数用符号q表示，是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。定义四元数如下所示。
这里直接给出结论，不作证明。会用即可。四元数与欧拉角的转换公式为：
用方向余弦表示欧拉角，这里欧拉角不允许等于90度。
用四元数表示欧拉角
在姿态解算中常用的算法由欧拉角法，方向余弦法和四元数法，欧拉角在求解姿态时存在奇点，无法用于全姿态结算，方向余弦没有奇点，但是计算量大，无法满足实时性要求，四元数法，计算量小，无奇点可以满足飞行器运动过程中姿态的实时解算，姿态解算的原理是对于一个确定的向量，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，以此来修正姿态。
19.3 实验例程实验内容：利用mpu6050采集到数据获取欧拉角显示在tftlcd上。
19.3.1 mpu6050内部相关寄存器（1） 电源管理寄存器1 （地址0x6b）
76543210
device_rst sleep cycle - temp_dis clksel[2:0]
bit 7：软件复位
0：不复位mpu6050
1：复位mpu6050
bit 6：休眠模式
0：正常工作模式
1：睡眠模式
bit 5：循环模式
0：默认状态
1：睡眠模式与唤醒模式交替运行
bit 3：温度传感器使能
0：使能温度传感器
1：禁用温度传感器
bit 2~bit 0：选择系统时钟源
000：内部8m rc时钟源
001：pll，使用x轴陀螺作为参考
010：pll，使用y轴陀螺作为参考
011：pll，使用z轴陀螺作为参考
100：pll，使用外部32.768khz作为参考
101：pll，使用外部19.2mhz作为参考
110：保留
111：关闭时钟，保持时序产生电路复位状态
（2） 陀螺仪配置寄存器 （地址：0x1b）
76543210
xg_st yg_st zg_st fs_sel[1:0] - - -
bit 7：陀螺仪x轴自检
0：禁用
1：启用
bit 6：陀螺仪y轴自检
0：禁用
1：启用
bit 5：陀螺仪z轴自检
0：禁用
1：启用
bit 4~bit 3：陀螺仪满量程
0：±250°/s
1：±500°/s
2：±1000°/s
3：±2000°/s
（3） 加速度传感器配置寄存器 （地址：0x1c）
76543210
xa_st ya_st za_st afs_sel[1:0] - - -
bit 7：加速度计x轴自检
0：禁用
1：启用
bit 6：加速度计y轴自检
0：禁用
1：启用
bit 5：加速度计z轴自检
0：禁用
1：启用
bit 4~bit 3：加速度传感器满量程
0：±2g
1：±4g
2：±8g
3：±16g
（4） fifo使能寄存器 （地址：0x23）
76543210
temp xg yg zg accel slv2 slv1 slv0
bit 7：temp_out_h和temp_out_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 6：gyro_xout_h和gyro_xout_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 5：gyro_yout_h和gyro_yout_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 4：gyro_zout_h和gyro_zout_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 3：accel_xout_h/ l，accel_yout_h/l，accel_zout_h/ l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 2：ext_sens_data寄存器和从机2缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 1：ext_sens_data寄存器和从机1缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 0：ext_sens_data寄存器和从机0缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
（5） 陀螺仪采样率分频寄存器 （地址：0x19）
76543210
smplrt_div[7:0]
采样频率=陀螺仪输出频率/（1+smplrt_div）
（6） 配置寄存器 （地址：0x1a）
76543210
- - ext_sync_set[2:0] dlpf_cfg[2:0]
bit 5~bit 3：该段内的值确定采样的值将代替传感器数据寄存器中的最低有效位
0：输入禁用
1：temp_out_l寄存器第0位
2：gyro_xout_l寄存器第0位
3：gyro_yout_l寄存器第0位
4：gyro_zout_l寄存器第0位
5：accel_xout_l寄存器第0位
6：accel _yout_l寄存器第0位
7：accel _zout_l寄存器第0位
bit 2~bit 0：低通滤波器设置
值加速度传感器（fs=1khz）角速度传感器
带宽（hz） 延迟（ms） 带宽（hz）
000 260 0
001 184 2.0
010 94 3.0
011 44 4.9
100 21 8.5
101 10 13.8
110 5 19.0
111 保留 保留
（7） 电源管理寄存器2 （地址：0x6c）
76543210
lp_wake_ctrl[1:0] stby_xa stby_ya stby_za stby_xg stby_yg xtby_zg
bit 7~bit 6：低功耗模式下的唤醒频率
0：1.25hz
1：5hz
2：20hz
3：40hz
bit 5：x轴加速度待机模式
0：禁用
1：启用
bit 4：y轴加速度待机模式
0：禁用
1：启用
bit 3：z轴加速度待机模式
0：禁用
1：启用
bit 2：x轴陀螺仪待机模式
0：禁用
1：启用
bit 1：y轴陀螺仪待机模式
0：禁用
1：启用
bit 0：z轴陀螺仪待机模式
0：禁用
1：启用
19.3.2 源代码（1）创建mpu6050.h文件，输入以下代码。
/********************************************************************************************************* mup6050 驱 动 文 件*********************************************************************************************************/#ifndef _mpu6050_h_#define _mpu6050_h_#include sys.h/********************************************************************************************************* 硬 件 端 口 定 义*********************************************************************************************************/#define mpu_iic_scl pbout( 10) //scl#define mpu_iic_sda pbout( 11 ) //sda#define mpu_read_sda pbin( 11 ) //输入sda#define mpu_ad0_ctrl paout( 15 ) //控制ad0电平,从而控制mpu地址/********************************************************************************************************* 数 据 结 构 定 义*********************************************************************************************************///如果ad0脚(9脚)接地,iic地址为0x68(不包含最低位)//如果接v3.3,则iic地址为0x69(不包含最低位)#define mpu_addr 0x68#define mpu_accel_offs_reg 0x06 //accel_offs寄存器,可读取版本号,寄存器手册未提到#define mpu_prod_id_reg 0x0c //prod id寄存器,在寄存器手册未提到#define mpu_self_testx_reg 0x0d //自检寄存器x#define mpu_self_testy_reg 0x0e //自检寄存器y#define mpu_self_testz_reg 0x0f //自检寄存器z#define mpu_self_testa_reg 0x10 //自检寄存器a#define mpu_sample_rate_reg 0x19 //采样频率分频器#define mpu_cfg_reg 0x1a //配置寄存器#define mpu_gyro_cfg_reg 0x1b //陀螺仪配置寄存器#define mpu_accel_cfg_reg 0x1c //加速度计配置寄存器#define mpu_motion_det_reg 0x1f //运动检测阀值设置寄存器#define mpu_fifo_en_reg 0x23 //fifo使能寄存器#define mpu_i2cmst_ctrl_reg 0x24 //iic主机控制寄存器#define mpu_i2cslv0_addr_reg 0x25 //iic从机0器件地址寄存器#define mpu_i2cslv0_reg 0x26 //iic从机0数据地址寄存器#define mpu_i2cslv0_ctrl_reg 0x27 //iic从机0控制寄存器#define mpu_i2cslv1_addr_reg 0x28 //iic从机1器件地址寄存器#define mpu_i2cslv1_reg 0x29 //iic从机1数据地址寄存器#define mpu_i2cslv1_ctrl_reg 0x2a //iic从机1控制寄存器#define mpu_i2cslv2_addr_reg 0x2b //iic从机2器件地址寄存器#define mpu_i2cslv2_reg 0x2c //iic从机2数据地址寄存器#define mpu_i2cslv2_ctrl_reg 0x2d //iic从机2控制寄存器#define mpu_i2cslv3_addr_reg 0x2e //iic从机3器件地址寄存器#define mpu_i2cslv3_reg 0x2f //iic从机3数据地址寄存器#define mpu_i2cslv3_ctrl_reg 0x30 //iic从机3控制寄存器#define mpu_i2cslv4_addr_reg 0x31 //iic从机4器件地址寄存器#define mpu_i2cslv4_reg 0x32 //iic从机4数据地址寄存器#define mpu_i2cslv4_do_reg 0x33 //iic从机4写数据寄存器#define mpu_i2cslv4_ctrl_reg 0x34 //iic从机4控制寄存器#define mpu_i2cslv4_di_reg 0x35 //iic从机4读数据寄存器#define mpu_i2cmst_sta_reg 0x36 //iic主机状态寄存器#define mpu_intbp_cfg_reg 0x37 //中断/旁路设置寄存器#define mpu_int_en_reg 0x38 //中断使能寄存器#define mpu_int_sta_reg 0x3a //中断状态寄存器#define mpu_accel_xouth_reg 0x3b //加速度值,x轴高8位寄存器#define mpu_accel_xoutl_reg 0x3c //加速度值,x轴低8位寄存器#define mpu_accel_youth_reg 0x3d //加速度值,y轴高8位寄存器#define mpu_accel_youtl_reg 0x3e //加速度值,y轴低8位寄存器#define mpu_accel_zouth_reg 0x3f //加速度值,z轴高8位寄存器#define mpu_accel_zoutl_reg 0x40 //加速度值,z轴低8位寄存器#define mpu_temp_outh_reg 0x41 //温度值高八位寄存器#define mpu_temp_outl_reg 0x42 //温度值低8位寄存器#define mpu_gyro_xouth_reg 0x43 //陀螺仪值,x轴高8位寄存器#define mpu_gyro_xoutl_reg 0x44 //陀螺仪值,x轴低8位寄存器#define mpu_gyro_youth_reg 0x45 //陀螺仪值,y轴高8位寄存器#define mpu_gyro_youtl_reg 0x46 //陀螺仪值,y轴低8位寄存器#define mpu_gyro_zouth_reg 0x47 //陀螺仪值,z轴高8位寄存器#define mpu_gyro_zoutl_reg 0x48 //陀螺仪值,z轴低8位寄存器#define mpu_i2cslv0_do_reg 0x63 //iic从机0数据寄存器#define mpu_i2cslv1_do_reg 0x64 //iic从机1数据寄存器#define mpu_i2cslv2_do_reg 0x65 //iic从机2数据寄存器#define mpu_i2cslv3_do_reg 0x66 //iic从机3数据寄存器#define mpu_i2cmst_delay_reg 0x67 //iic主机延时管理寄存器#define mpu_sigpath_rst_reg 0x68 //信号通道复位寄存器#define mpu_mdetect_ctrl_reg 0x69 //运动检测控制寄存器#define mpu_user_ctrl_reg 0x6a //用户控制寄存器#define mpu_pwr_mgmt1_reg 0x6b //电源管理寄存器1#define mpu_pwr_mgmt2_reg 0x6c //电源管理寄存器2 #define mpu_fifo_cnth_reg 0x72 //fifo计数寄存器高八位#define mpu_fifo_cntl_reg 0x73 //fifo计数寄存器低八位#define mpu_fifo_rw_reg 0x74 //fifo读写寄存器#define mpu_device_id_reg 0x75 //器件id寄存器/********************************************************************************************************* 函 数 列 表*********************************************************************************************************/void mpu_iic_init( void ) ; //初始化iicu8 mpu_init( void ) ; //初始化mpu6050u8 mpu_read_len( u8 addr, u8 reg, u8 len, u8 *buf ) ; //iic连续读u8 mpu_write_len( u8 addr, u8 reg, u8 len, u8 *buf ) ; //iic连续写short mpu_get_temperature( void ) ; //获取温度u8 mpu_get_gyroscope( short *gx, short *gy, short *gz ) ; //获取陀螺仪值u8 mpu_get_accelerometer( short *ax, short *ay, short *az ) ; //获取加速度值#endif（2）创建mpu6050.c文件，输入以下代码。
/********************************************************************************************************* mup6050 驱 动 程 序*********************************************************************************************************/#include mpu6050.h#include delay.h/***************************************************name :mpu_iic_initfunction :初始化iicparamater :nonereturn :none***************************************************/void mpu_iic_init(){ rcc->apb2enr |= 1crh |= 0x00003300 ; gpiob->odr |= 3crh |= 0x00003000 ; mpu_iic_sda = 1 ; mpu_iic_scl = 1 ; delay_us( 2 ) ; mpu_iic_sda = 0 ; delay_us( 2 ) ; mpu_iic_scl = 0 ;}/***************************************************name :mpu_iic_wait_ackfunction :停止时序paramater :nonereturn :none***************************************************/void mpu_iic_stop(){ gpiob->crh &= 0xffff0fff ; gpiob->crh |= 0x00003000 ; mpu_iic_scl = 0 ; mpu_iic_sda = 0 ; delay_us( 2 ) ; mpu_iic_scl = 1 ; mpu_iic_sda = 1 ; delay_us( 2 ) ;}/***************************************************name :mpu_iic_wait_ackfunction :应答时序paramater :nonereturn : 0:成功 1:失败***************************************************/u8 mpu_iic_wait_ack(){ u8 ucerrtime=0 ; gpiob->crh &= 0xffff0fff ; gpiob->crh |= 0x00008000 ; mpu_iic_sda = 1 ; delay_us( 2 ) ; mpu_iic_scl = 1 ; delay_us( 2 ) ; while( mpu_read_sda ) { ucerrtime ++ ; if( ucerrtime>250 ) { mpu_iic_stop() ; return 1 ; } } mpu_iic_scl = 0 ; //时钟输出0 return 0 ;}/***************************************************name :mpu_iic_send_bytefunction :iic发送1个字节paramater : ack:应答控制 0:不应答 1:应答return :none***************************************************/void mpu_iic_send_byte( u8 byte ){ u8 i ; gpiob->crh &= 0xffff0fff ; gpiob->crh |= 0x00003000 ; mpu_iic_scl = 0 ; //拉低时钟开始数据传输 for( i=0; i<8; i++ ) { if( ( byte&0x80 )==0x80 ) mpu_iic_sda = 1 ; else mpu_iic_sda = 0 ; byte crh |= 0x00008000 ; for( i=0; i<8; i++ ) { mpu_iic_scl = 0 ; delay_us( 2 ) ; mpu_iic_scl = 1 ; byte crh |= 0x00003000 ; mpu_iic_sda = 1-ack ; delay_us( 2 ) ; mpu_iic_scl = 1 ; delay_us( 2 ) ; mpu_iic_scl = 0 ; return byte ;}/***************************************************name :mpu_write_bytefunction :iic写一个字节paramater : reg:寄存器地址 data:数据return : 0:正常 其他:错误代码***************************************************/u8 mpu_write_byte( u8 reg, u8 data ){ mpu_iic_start() ; mpu_iic_send_byte( mpu_addr<<1 ) ; //发送器件地址+写命令 //等待应答 if( mpu_iic_wait_ack() ) { mpu_iic_stop() ; return 1 ; } mpu_iic_send_byte( reg ) ; //写寄存器地址 mpu_iic_wait_ack() ; //等待应答 mpu_iic_send_byte( data ) ; //发送数据 //等待ack if( mpu_iic_wait_ack() ) { mpu_iic_stop() ; return 1 ; } mpu_iic_stop() ; return 0 ;}/***************************************************name :mpu_read_bytefunction :iic读一个字节paramater : reg:寄存器地址return :读到的数据***************************************************/u8 mpu_read_byte( u8 reg ){ u8 res ; mpu_iic_start() ; mpu_iic_send_byte( mpu_addr<<1 ) ; //发送器件地址+写命令 mpu_iic_wait_ack() ; //等待应答 mpu_iic_send_byte( reg ) ; //写寄存器地址 mpu_iic_wait_ack() ; //等待应答 mpu_iic_start() ; mpu_iic_send_byte( ( mpu_addr<1000 ) rate=1000 ; if( rate=188 ) data = 1 ; else if( ( rate/2 )>=98 ) data = 2 ; else if( ( rate/2 )>=42 ) data = 3 ; else if( ( rate/2 )>=20 ) data = 4; else if( ( rate/2 )>=10 ) data = 5 ; else data = 6 ; return mpu_write_byte( mpu_cfg_reg, data ) ; //设置数字低通滤波器}/***************************************************name :mpu_initfunction :初始化mpu6050paramater :nonereturn : 0:成功 其他:错误代码***************************************************/u8 mpu_init(){ u8 res ; rcc->apb2enr |= 1crh |= 0x30000000 ; jtag_set( 1 ) ; //禁止jtag,从而pa15可以做普通io使用,否则pa15不能做普通io mpu_ad0_ctrl = 0 ; //控制mpu6050的ad0脚为低电平,从机地址为:0x68 //初始化iic总线 rcc->apb2enr |= 1crh |= 0x00003300 ; gpiob->odr |= 3<<10 ; //pb10,11 输出高 mpu_write_byte( mpu_pwr_mgmt1_reg, 0x80 ) ; //复位mpu6050 delay_ms( 100 ) ; mpu_write_byte( mpu_pwr_mgmt1_reg, 0x00 ) ; //唤醒mpu6050 mpu_write_byte( mpu_gyro_cfg_reg, 3<<3 ) ; //陀螺仪传感器,±2000dps mpu_write_byte( mpu_accel_cfg_reg, 0<<3 ) ; //加速度传感器,±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 ) ; //器件id正确 if( res==mpu_addr ) { 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 ;}/***************************************************name :mpu_write_lenfunction :iic连续写paramater : addr:器件地址 reg:寄存器地址 len:写入长度 buf:数据区return : 0:成功 其他:错误代码***************************************************/u8 mpu_write_len( u8 addr, u8 reg, u8 len, u8 *buf ){ u8 i ; mpu_iic_start() ; mpu_iic_send_byte( addr<<1 ) ; //发送器件地址+写命令 if( mpu_iic_wait_ack() ) //等待应答 { mpu_iic_stop() ; return 1 ; } mpu_iic_send_byte( reg ) ; //写寄存器地址 mpu_iic_wait_ack() ; //等待应答 for( i=0; i（3）创建1.c文件，输入以下代码。
#include sys.h#include delay.h#include usart1.h#include lcd.h#include mpu6050.h#include inv_mpu.h#include inv_mpu_dmp_motion_driver.hint main(){ u8 t, str[ 20 ] ; float pitch, roll, yaw ; //欧拉角 short aacx, aacy, aacz ; //加速度传感器原始数据 short gyrox, gyroy, gyroz ; //陀螺仪原始数据 float temp ; //温度 stm32_clock_init( 9 ) ; //系统时钟设置 systick_init( 72 ) ; //延时初始化 usart1_init( 72, 500000 ) ; //串口初始化为500000 lcd_init() ; //初始化lcd mpu_init() ; //初始化mpu6050 while( mpu_dmp_init() ) ; point_color = red ; //设置字体为蓝色 while(1) { if( mpu_dmp_get_data( &pitch, &roll, &yaw )==0 ) { temp = ( float )mpu_get_temperature()/100 ; //得到温度值 mpu_get_accelerometer( &aacx, &aacy, &aacz ) ; //得到加速度传感器数据 mpu_get_gyroscope( &gyrox, &gyroy, &gyroz ) ; //得到陀螺仪数据 //转换温度 sprintf( ( char* )str, temp: %.2f c, temp ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 4 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 0, str ) ; //自转角 sprintf( ( char* )str, pitch: %.1f c, pitch ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 3 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 30, str ) ; //章动角 sprintf( ( char* )str, roll: %.1f c, roll ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 3 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 60, str ) ; //旋转角 sprintf( ( char* )str, yaw: %.1f c, yaw ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 3 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 90, str ) ; } }}注：例程使用了网上已经移植成功的dmp源码，直接调用即可。
HDIO OBUFT和IOBUF用例简析
LDO稳压器如何才能实现输出电压的稳定和
图像传感器在数码上的应用
揭秘纳米制程工艺背后的虚与实
5G时代再次迎来高峰发展，射频连接器厂商如何把握机会？
MPU6050传感器解析实验
升腾910和310哪个更好？
F007运放组成的精密限幅放大器
淘宝又过节 移动支付卡位战开打
更广泛领域的AI应用
苹果开放降级通道是真是假_苹果降级通道怎么查询_苹果系统降级通道查询教程
Type-C集线器厂商提高高速网络的产品
微雪电子树莓派4代B显示配件包介绍
Intel退出移动市场？此话言之过早！
全通滤波器工作原理及移相原理
小米运动蓝牙耳机青春版拆解：99元超值
上海浦东智能照明联合会第一届第八次理事会在上海浦东召开
正式打响KA渠道战，智伴科技与新华书店、沃尔玛等众多知名品牌达成合作
在VSCode中使用J-Link调试嵌入式系统
焊盘与丝网图形
19.1.2 引脚介绍
mpu6050采用qfn-24封装，端口描述如下表所示。
引脚编号引脚名称功能
1 clkin 外部参考时钟输入，如果不使用直接接地
2 nc 空引脚
3 nc 空引脚
4 nc 空引脚
5 nc 空引脚
6 aux_da 从i2c接口数据口，用于连接磁传感器的sda组成九轴传感器
7 aux_cl 从i2c接口时钟口，用于连接磁传感器的scl组成九轴传感器
8 vlogic io口逻辑电平，最低可以设置1.8v，默认连接vdd
9 ad0 i2c接口地址控制端，端口为高电平默认地址0x69，端口为低电平默认地址0x68
10 regout 外接稳压器的滤波电容
11 fsync 帧同步数字输入，如果不使用直接接gnd
12 int 中断信号输出（可以配置为开漏输出）
13 vdd 电源正极，供电范围0.5v~6vdc
14 nc 空引脚
15 nc 空引脚
16 nc 空引脚
17 nc 空引脚
18 gnd 电源地
19 resv 保留
20 cpout 外部电荷泵电容
21 resv 保留
22 resv 保留
23 scl 主i2c接口时钟
24 sda 主i2c接口数据
19.1.3 硬件电路
由于mpu6050内部是可以自动计算x，y和z轴的方向及加速度的，使用者可以不考虑实际的数据转换问题，但是为了详细的了解mpu6050的计算过程，使用者最好还是应该具备了解原始数据如何转换为我们需要的角度与加速度值。
19.2 姿态解算与融合算法基础概念19.2.1 方向矩阵设有一个三位直角坐标系oxyz，如下图所示。
19.2.2 方向余弦矩阵
19.2.3 欧拉角欧拉角是用于确定定点转动缸体位置的3个1组的独立角参量，由章动角θ，旋转角（进动角）ψ和自转角φ组成，欧拉角有多种取法，下面是比较常见的一种。
如上图所示，由定点o做出固定坐标系oxyz以及固定连在刚体的坐标系ox’y’z’，以轴oz和oz’为基本轴。其垂直面oxy和ox’y’为基本平面，由轴oz量到oz’的角度θ称为章动角，平面zoz’的垂线on称为节线，同时on又是基本平面ox’y’和oxy的交线，在右手坐标系中，由on的正端看，角θ应按照逆时针方向计算，由固定轴ox到节线on的角度ψ称为进动角，也叫作旋转角，由节线on到动轴ox’的角度φ称为自转角，有oz和oz’正端看，进动角ψ与自转角φ也应该按照逆时针方向计算。
从上面的描述过程可以发现，欧拉角实际是可以分解成三步来计算的：
第1步：绕z轴旋转α，使得x轴与n轴重合
第2步：绕x轴旋转β，使z轴与旋转后的z轴重合
第3步：绕z轴旋转y，是坐标系与旋转后的完全重合
根据上面的三个步骤，我们来通过以下实例来说明欧拉角与方向余弦矩阵的转换过程。
19.2.4 四元数与欧拉角的转换四元数是一个简单的超复数，是由实数加上三个虚数单位i，j，k组成，每个四元数都是1，i，j，k的线性组合，四元数是爱尔兰数学家哈密顿在1843年发明的数学概念，四元数的乘法不符合交换律。
四元数姿态表达式是一个四参数的表达式，它的基本思路是一个坐标系转换到另一个坐标系可以通过绕一个定义在参考系中的矢量μ的单次转动来实现，四元数用符号q表示，是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。定义四元数如下所示。
这里直接给出结论，不作证明。会用即可。四元数与欧拉角的转换公式为：
用方向余弦表示欧拉角，这里欧拉角不允许等于90度。
用四元数表示欧拉角
在姿态解算中常用的算法由欧拉角法，方向余弦法和四元数法，欧拉角在求解姿态时存在奇点，无法用于全姿态结算，方向余弦没有奇点，但是计算量大，无法满足实时性要求，四元数法，计算量小，无奇点可以满足飞行器运动过程中姿态的实时解算，姿态解算的原理是对于一个确定的向量，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，以此来修正姿态。
19.3 实验例程实验内容：利用mpu6050采集到数据获取欧拉角显示在tftlcd上。
19.3.1 mpu6050内部相关寄存器（1） 电源管理寄存器1 （地址0x6b）
76543210
device_rst sleep cycle - temp_dis clksel[2:0]
bit 7：软件复位
0：不复位mpu6050
1：复位mpu6050
bit 6：休眠模式
0：正常工作模式
1：睡眠模式
bit 5：循环模式
0：默认状态
1：睡眠模式与唤醒模式交替运行
bit 3：温度传感器使能
0：使能温度传感器
1：禁用温度传感器
bit 2~bit 0：选择系统时钟源
000：内部8m rc时钟源
001：pll，使用x轴陀螺作为参考
010：pll，使用y轴陀螺作为参考
011：pll，使用z轴陀螺作为参考
100：pll，使用外部32.768khz作为参考
101：pll，使用外部19.2mhz作为参考
110：保留
111：关闭时钟，保持时序产生电路复位状态
（2） 陀螺仪配置寄存器 （地址：0x1b）
76543210
xg_st yg_st zg_st fs_sel[1:0] - - -
bit 7：陀螺仪x轴自检
0：禁用
1：启用
bit 6：陀螺仪y轴自检
0：禁用
1：启用
bit 5：陀螺仪z轴自检
0：禁用
1：启用
bit 4~bit 3：陀螺仪满量程
0：±250°/s
1：±500°/s
2：±1000°/s
3：±2000°/s
（3） 加速度传感器配置寄存器 （地址：0x1c）
76543210
xa_st ya_st za_st afs_sel[1:0] - - -
bit 7：加速度计x轴自检
0：禁用
1：启用
bit 6：加速度计y轴自检
0：禁用
1：启用
bit 5：加速度计z轴自检
0：禁用
1：启用
bit 4~bit 3：加速度传感器满量程
0：±2g
1：±4g
2：±8g
3：±16g
（4） fifo使能寄存器 （地址：0x23）
76543210
temp xg yg zg accel slv2 slv1 slv0
bit 7：temp_out_h和temp_out_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 6：gyro_xout_h和gyro_xout_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 5：gyro_yout_h和gyro_yout_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 4：gyro_zout_h和gyro_zout_l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 3：accel_xout_h/ l，accel_yout_h/l，accel_zout_h/ l缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 2：ext_sens_data寄存器和从机2缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 1：ext_sens_data寄存器和从机1缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
bit 0：ext_sens_data寄存器和从机0缓冲区激活
0：关闭该缓冲区
1：激活该fifo缓冲区
（5） 陀螺仪采样率分频寄存器 （地址：0x19）
76543210
smplrt_div[7:0]
采样频率=陀螺仪输出频率/（1+smplrt_div）
（6） 配置寄存器 （地址：0x1a）
76543210
- - ext_sync_set[2:0] dlpf_cfg[2:0]
bit 5~bit 3：该段内的值确定采样的值将代替传感器数据寄存器中的最低有效位
0：输入禁用
1：temp_out_l寄存器第0位
2：gyro_xout_l寄存器第0位
3：gyro_yout_l寄存器第0位
4：gyro_zout_l寄存器第0位
5：accel_xout_l寄存器第0位
6：accel _yout_l寄存器第0位
7：accel _zout_l寄存器第0位
bit 2~bit 0：低通滤波器设置
值加速度传感器（fs=1khz）角速度传感器
带宽（hz） 延迟（ms） 带宽（hz）
000 260 0
001 184 2.0
010 94 3.0
011 44 4.9
100 21 8.5
101 10 13.8
110 5 19.0
111 保留 保留
（7） 电源管理寄存器2 （地址：0x6c）
76543210
lp_wake_ctrl[1:0] stby_xa stby_ya stby_za stby_xg stby_yg xtby_zg
bit 7~bit 6：低功耗模式下的唤醒频率
0：1.25hz
1：5hz
2：20hz
3：40hz
bit 5：x轴加速度待机模式
0：禁用
1：启用
bit 4：y轴加速度待机模式
0：禁用
1：启用
bit 3：z轴加速度待机模式
0：禁用
1：启用
bit 2：x轴陀螺仪待机模式
0：禁用
1：启用
bit 1：y轴陀螺仪待机模式
0：禁用
1：启用
bit 0：z轴陀螺仪待机模式
0：禁用
1：启用
19.3.2 源代码（1）创建mpu6050.h文件，输入以下代码。
/********************************************************************************************************* mup6050 驱 动 文 件*********************************************************************************************************/#ifndef _mpu6050_h_#define _mpu6050_h_#include sys.h/********************************************************************************************************* 硬 件 端 口 定 义*********************************************************************************************************/#define mpu_iic_scl pbout( 10) //scl#define mpu_iic_sda pbout( 11 ) //sda#define mpu_read_sda pbin( 11 ) //输入sda#define mpu_ad0_ctrl paout( 15 ) //控制ad0电平,从而控制mpu地址/********************************************************************************************************* 数 据 结 构 定 义*********************************************************************************************************///如果ad0脚(9脚)接地,iic地址为0x68(不包含最低位)//如果接v3.3,则iic地址为0x69(不包含最低位)#define mpu_addr 0x68#define mpu_accel_offs_reg 0x06 //accel_offs寄存器,可读取版本号,寄存器手册未提到#define mpu_prod_id_reg 0x0c //prod id寄存器,在寄存器手册未提到#define mpu_self_testx_reg 0x0d //自检寄存器x#define mpu_self_testy_reg 0x0e //自检寄存器y#define mpu_self_testz_reg 0x0f //自检寄存器z#define mpu_self_testa_reg 0x10 //自检寄存器a#define mpu_sample_rate_reg 0x19 //采样频率分频器#define mpu_cfg_reg 0x1a //配置寄存器#define mpu_gyro_cfg_reg 0x1b //陀螺仪配置寄存器#define mpu_accel_cfg_reg 0x1c //加速度计配置寄存器#define mpu_motion_det_reg 0x1f //运动检测阀值设置寄存器#define mpu_fifo_en_reg 0x23 //fifo使能寄存器#define mpu_i2cmst_ctrl_reg 0x24 //iic主机控制寄存器#define mpu_i2cslv0_addr_reg 0x25 //iic从机0器件地址寄存器#define mpu_i2cslv0_reg 0x26 //iic从机0数据地址寄存器#define mpu_i2cslv0_ctrl_reg 0x27 //iic从机0控制寄存器#define mpu_i2cslv1_addr_reg 0x28 //iic从机1器件地址寄存器#define mpu_i2cslv1_reg 0x29 //iic从机1数据地址寄存器#define mpu_i2cslv1_ctrl_reg 0x2a //iic从机1控制寄存器#define mpu_i2cslv2_addr_reg 0x2b //iic从机2器件地址寄存器#define mpu_i2cslv2_reg 0x2c //iic从机2数据地址寄存器#define mpu_i2cslv2_ctrl_reg 0x2d //iic从机2控制寄存器#define mpu_i2cslv3_addr_reg 0x2e //iic从机3器件地址寄存器#define mpu_i2cslv3_reg 0x2f //iic从机3数据地址寄存器#define mpu_i2cslv3_ctrl_reg 0x30 //iic从机3控制寄存器#define mpu_i2cslv4_addr_reg 0x31 //iic从机4器件地址寄存器#define mpu_i2cslv4_reg 0x32 //iic从机4数据地址寄存器#define mpu_i2cslv4_do_reg 0x33 //iic从机4写数据寄存器#define mpu_i2cslv4_ctrl_reg 0x34 //iic从机4控制寄存器#define mpu_i2cslv4_di_reg 0x35 //iic从机4读数据寄存器#define mpu_i2cmst_sta_reg 0x36 //iic主机状态寄存器#define mpu_intbp_cfg_reg 0x37 //中断/旁路设置寄存器#define mpu_int_en_reg 0x38 //中断使能寄存器#define mpu_int_sta_reg 0x3a //中断状态寄存器#define mpu_accel_xouth_reg 0x3b //加速度值,x轴高8位寄存器#define mpu_accel_xoutl_reg 0x3c //加速度值,x轴低8位寄存器#define mpu_accel_youth_reg 0x3d //加速度值,y轴高8位寄存器#define mpu_accel_youtl_reg 0x3e //加速度值,y轴低8位寄存器#define mpu_accel_zouth_reg 0x3f //加速度值,z轴高8位寄存器#define mpu_accel_zoutl_reg 0x40 //加速度值,z轴低8位寄存器#define mpu_temp_outh_reg 0x41 //温度值高八位寄存器#define mpu_temp_outl_reg 0x42 //温度值低8位寄存器#define mpu_gyro_xouth_reg 0x43 //陀螺仪值,x轴高8位寄存器#define mpu_gyro_xoutl_reg 0x44 //陀螺仪值,x轴低8位寄存器#define mpu_gyro_youth_reg 0x45 //陀螺仪值,y轴高8位寄存器#define mpu_gyro_youtl_reg 0x46 //陀螺仪值,y轴低8位寄存器#define mpu_gyro_zouth_reg 0x47 //陀螺仪值,z轴高8位寄存器#define mpu_gyro_zoutl_reg 0x48 //陀螺仪值,z轴低8位寄存器#define mpu_i2cslv0_do_reg 0x63 //iic从机0数据寄存器#define mpu_i2cslv1_do_reg 0x64 //iic从机1数据寄存器#define mpu_i2cslv2_do_reg 0x65 //iic从机2数据寄存器#define mpu_i2cslv3_do_reg 0x66 //iic从机3数据寄存器#define mpu_i2cmst_delay_reg 0x67 //iic主机延时管理寄存器#define mpu_sigpath_rst_reg 0x68 //信号通道复位寄存器#define mpu_mdetect_ctrl_reg 0x69 //运动检测控制寄存器#define mpu_user_ctrl_reg 0x6a //用户控制寄存器#define mpu_pwr_mgmt1_reg 0x6b //电源管理寄存器1#define mpu_pwr_mgmt2_reg 0x6c //电源管理寄存器2 #define mpu_fifo_cnth_reg 0x72 //fifo计数寄存器高八位#define mpu_fifo_cntl_reg 0x73 //fifo计数寄存器低八位#define mpu_fifo_rw_reg 0x74 //fifo读写寄存器#define mpu_device_id_reg 0x75 //器件id寄存器/********************************************************************************************************* 函 数 列 表*********************************************************************************************************/void mpu_iic_init( void ) ; //初始化iicu8 mpu_init( void ) ; //初始化mpu6050u8 mpu_read_len( u8 addr, u8 reg, u8 len, u8 *buf ) ; //iic连续读u8 mpu_write_len( u8 addr, u8 reg, u8 len, u8 *buf ) ; //iic连续写short mpu_get_temperature( void ) ; //获取温度u8 mpu_get_gyroscope( short *gx, short *gy, short *gz ) ; //获取陀螺仪值u8 mpu_get_accelerometer( short *ax, short *ay, short *az ) ; //获取加速度值#endif（2）创建mpu6050.c文件，输入以下代码。
/********************************************************************************************************* mup6050 驱 动 程 序*********************************************************************************************************/#include mpu6050.h#include delay.h/***************************************************name :mpu_iic_initfunction :初始化iicparamater :nonereturn :none***************************************************/void mpu_iic_init(){ rcc->apb2enr |= 1crh |= 0x00003300 ; gpiob->odr |= 3
#include sys.h#include delay.h#include usart1.h#include lcd.h#include mpu6050.h#include inv_mpu.h#include inv_mpu_dmp_motion_driver.hint main(){ u8 t, str[ 20 ] ; float pitch, roll, yaw ; //欧拉角 short aacx, aacy, aacz ; //加速度传感器原始数据 short gyrox, gyroy, gyroz ; //陀螺仪原始数据 float temp ; //温度 stm32_clock_init( 9 ) ; //系统时钟设置 systick_init( 72 ) ; //延时初始化 usart1_init( 72, 500000 ) ; //串口初始化为500000 lcd_init() ; //初始化lcd mpu_init() ; //初始化mpu6050 while( mpu_dmp_init() ) ; point_color = red ; //设置字体为蓝色 while(1) { if( mpu_dmp_get_data( &pitch, &roll, &yaw )==0 ) { temp = ( float )mpu_get_temperature()/100 ; //得到温度值 mpu_get_accelerometer( &aacx, &aacy, &aacz ) ; //得到加速度传感器数据 mpu_get_gyroscope( &gyrox, &gyroy, &gyroz ) ; //得到陀螺仪数据 //转换温度 sprintf( ( char* )str, temp: %.2f c, temp ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 4 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 0, str ) ; //自转角 sprintf( ( char* )str, pitch: %.1f c, pitch ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 3 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 30, str ) ; //章动角 sprintf( ( char* )str, roll: %.1f c, roll ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 3 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 60, str ) ; //旋转角 sprintf( ( char* )str, yaw: %.1f c, yaw ) ; for( t=0; t<20; t++ ) { if( str[ t ]=='.' ) { t += 3 ; while( t<20 ) { t ++ ; str[ t ] = ' ' ; } } } lcd_showstring( 10, 90, str ) ; } }}注：例程使用了网上已经移植成功的dmp源码，直接调用即可。
HDIO OBUFT和IOBUF用例简析
LDO稳压器如何才能实现输出电压的稳定和
图像传感器在数码上的应用
揭秘纳米制程工艺背后的虚与实
5G时代再次迎来高峰发展，射频连接器厂商如何把握机会？
MPU6050传感器解析实验
升腾910和310哪个更好？
F007运放组成的精密限幅放大器
淘宝又过节 移动支付卡位战开打
更广泛领域的AI应用
苹果开放降级通道是真是假_苹果降级通道怎么查询_苹果系统降级通道查询教程
Type-C集线器厂商提高高速网络的产品
微雪电子树莓派4代B显示配件包介绍
Intel退出移动市场？此话言之过早！
全通滤波器工作原理及移相原理
小米运动蓝牙耳机青春版拆解：99元超值
上海浦东智能照明联合会第一届第八次理事会在上海浦东召开
正式打响KA渠道战，智伴科技与新华书店、沃尔玛等众多知名品牌达成合作
在VSCode中使用J-Link调试嵌入式系统
焊盘与丝网图形