一、前言
1.1 引言
MPU-60X0 是世界上首款集成式 6 轴运动跟踪器件,它将**3 轴陀螺仪、3 轴加速计和数字运动处理器 (DMP)**集成在一个 4x4x0.9mm 的小型封装中。MPU6050如下所示:
本次DEMO的目标是使用STM32F103ZET6的软件I2C实现MPU6050的数据(包括3轴加速度、3轴角速度、温度以及DMP库姿态解算数据)读取。
1.2 前置知识及参考文档
本文的前置知识
STM32实现软件I2C:【STM32开发之寄存器版】(十二)-I2C基础知识详解
本文参考资料
MPU6050官方手册:MPU6050_datasheet
MPU6050寄存器手册:MPU6050_Register_Map
1.3 硬件接线
本文使用软件I2C实现MPU6050的数据读取,STM32F103ZET6与MPU6050的接线方式如下:
STM32F103ZET6 MPU6050 VCC VCC GND GND PB10 I2C_SCL PB11 I2C_SDA PA15(注意与SWD冲突) AD0
二、MPU6050原理
2.1 MPU6050系统框图
MPU6050的系统框图如下所示:
MPU6050中的3轴角速度数据、3轴加速度数据、温度数据都可以通过寄存器直接读取,但是姿态角数据(包括俯仰角pitch、横滚角roll,偏航角yaw)需要通过芯片内部自带的DMP(数字运动处理器)进行解算,由于DMP可以自动完成姿态解算,因此大大减少了CPU的占用率。
2.2 四元数解算欧拉角原理
**MPU6050自带的DMP库输出的是四元数,因此需要将其转化为欧拉角。**MPU6050的检测轴及方向如下图所示:
1. 欧拉角通常由三个角度表示
- 偏航角(Yaw):ψ(绕Z轴旋转)
- 俯仰角(Pitch):θ(绕Y轴旋转)
- 横滚角(Roll):φ(绕X轴旋转)
2. 四元数表示
其中,q0是实部,q1/q2/q3为虚部。
3. 计算每个旋转的四元数
绕Z轴旋转偏航角ψ:
绕Y轴旋转俯仰角θ:
绕X轴旋转横滚角φ:
4. 将四元数相乘得到最终的四元数
假设我们使用的是 ZYX 顺序的欧拉角(先偏航,再俯仰,最后横滚),按照 ZYX 顺序进行乘法:
具体的乘法公式为:
5. 从四元数到欧拉角的转换
偏航角(Yaw):
俯仰角(Pitch):
横滚角(Roll):
三、MPU6050初始化流程
在使用MPU6050之前,需要对MPU6050进行初始化,具体的初始化步骤如下所示:
3.1 初始化I2C接口
MPU6050通过软件I2C与STM32进行通讯,因此需要先初始化STM32的SDA和SCL引脚(由于是软件I2C,仅需配置成推挽输出)。为了方便I2C寻址,需要设置AD0为0,本文选择PA15控制AD0,在实际操作过程中,注意PA15作为GPIO的功能与SWD功能冲突。
3.2 复位并唤醒MPU6050
MPU6050的复位功能通过电源管理寄存器1**(0X6B)的bit7写1实现**,复位后,该寄存器恢复默认值0X40,然后必须设置该寄存器为0X00,以唤醒MPU6050,进入正常工作状态。
3.3 设置角速度计、加速度计满量程范围
这一步,我们设置两个传感器的满量程范围(FSR),分别通过陀螺仪配置寄存器(0X1B)和加速度传感器配置寄存器(0X1C)设置。我们一般设置陀螺仪的满量程范围为**±2000dps**,加速度传感器的满量程范围为**±2g**。
3.4 设置其他参数
除了上述寄存器参数,我们还需要配置的参数有:关闭中断、关闭 AUX IIC 接口、禁止 FIFO、设置陀螺仪采样率和设置数字低通滤波器(DLPF)等。本章我们不用中断方式读取数据,所以关闭中断,然后也没用到 AUX IIC 接口外接其他传感器,所以也关闭这个接口。分别通过中断使能寄存器(0X38)和用户控制寄存器(0X6A)控制。MPU6050 可以使用 FIFO 存储传感器数据,不过本章我们没有用到,所以关闭所有 FIFO 通道,这个通过 FIFO 使能寄存器(0X23)控制,默认都是 0(即禁止 FIFO),所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器(0X19)控制,这个采样率我们一般设置为 50 即可。数字低通滤波器(DLPF)则通过配置寄存器(0X1A)设置,一般设置 DLPF 为带宽的 1/2 即可。
3.5 配置系统时钟源并使能角速度计和加速度计
系统时钟源同样是通过电源管理寄存器 1(0X6B)来设置,该寄存器的最低三位用于设置系统时钟源选择,默认值是 0(内部 8M RC 震荡),不过我们一般设置为 1,**选择 x 轴陀螺 PLL作为时钟源,以获得更高精度的时钟。**同时,使能角速度传感器和加速度传感器,这两个操作通过电源管理寄存器 2(0X6C)来设置,设置对应位为 0 即可开启。
四、MPU6050寄存器介绍
根据第三部分对MPU6050初始化过程的介绍,本文涉及的寄存器主要包含以下几个:
寄存器 | 功能 |
---|---|
PWR_MGMT_1 | 电源管理寄存器1 |
GYRO_CONFIG | 陀螺仪配置寄存器 |
ACCEL_CONFIG | 加速度传感器配置寄存器 |
FIFO_EN | FIFO使能寄存器 |
SMPLRT_DIV | 陀螺仪采样率分频寄存器 |
CONFIG | 配置寄存器 |
PWR_MGMT_2 | 电源管理寄存器2 |
GYRO_X/Y/ZOUT_H/L | 陀螺仪数据输出寄存器 |
ACCEL_X/Y/ZOUT_H/L | 加速度传感器数据输出寄存器 |
TEMP_OUT_H/L | 温度数据输出寄存器 |
下面对这些寄存器进行一一介绍。
4.1 电源管理寄存器1
《MPU6050_Register_Map》对电源管理寄存器1的描述如下所示:
本寄存器复位值为0X40,我们需要关注以下几位:
[7]DEVICE_RESET:该位用来控制复位,设置为1复位,复位结束后,硬件自动清0.
[6]SLEEP:复位结束后该位值为1,即进入睡眠模式,需手动清0结束睡眠。
[3]TEMP_DIS:用于设置是否使能温度传感器,设置为0则使能。
[2:0]CLKSEL:用于选择系统时钟源,如下图所示。默认选择内部8M RC时钟,我们选择X轴陀螺作为参考(即设置为1),获得精度更高的时钟源。
4.2 陀螺仪配置寄存器
《MPU6050_Register_Map》对陀螺仪配置寄存器的描述如下所示:
我们需要关注以下几位:
**[4:3]FS_SEL:**用于设置陀螺仪的满量程范围0,±250°/S;1,±500°/S;2,±1000°/S;3,±2000°/S;我们一般设置为 3,即±2000°/S,因为陀螺仪的 ADC 为 16 位分辨率,所以得到灵敏度为:65536/4000=16.4LSB/(°/S)。
4.3 加速度传感器配置寄存器
《MPU6050_Register_Map》对加速度传感器配置寄存器的描述如下所示:
我们需要关注以下几位:
**[4:3]AFS_SEL:**用于设置加速度传感器的满量程范围0,±2g;1,±4g;2,±8g;3,±16g;我们一般设置为 0,即±2g,因为加速度传感器的 ADC也是 16 位,所以得到灵敏度为:65536/4=16384LSB/g。
4.4 FIFO使能寄存器
《MPU6050_Register_Map》对FIFO使能寄存器的描述如下所示:
该寄存器用于控制 FIFO 使能,在简单读取传感器数据的时候,可以不用 FIFO,设置对应位为 0 即可禁止 FIFO,**设置为 1,则使能 FIFO。**注意加速度传感器的 3 个轴,全由 1 个位(ACCEL_FIFO_EN)控制,只要该位置 1,则加速度传感器的三个通道都开启 FIFO 了。
4.5 陀螺仪采样率分频寄存器
《MPU6050_Register_Map》对陀螺仪采样率分频寄存器的描述如下所示:
该寄存器用于设置MPU6050的陀螺仪采样频率,计算公式为:
采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)
这里陀螺仪的输出频率,是 1Khz 或者 8Khz,与数字低通滤波器(DLPF)的设置有关,当 DLPF_CFG=0/7 的时候,频率为 8Khz,其他情况是 1Khz。而且 DLPF 滤波频率一般设置为采样率的一半。采样率,我们假定设置为 50Hz,那么 SMPLRT_DIV=1000/50-1=19。
4.6 配置寄存器
《MPU6050_Register_Map》对配置寄存器的描述如下所示:
我们需要关注以下位:
**[2:0]DLPF_CFG:**数字低通滤波器(DLPF)的设置位,加速度计和陀螺仪,都是根据这三个位的配置进行过滤的。DLPF_CFG 不同配置对应的过滤情况如下表所示:
这里的加速度传感器,输出速率(Fs)固定是 1Khz,而角速度传感器的输出速率(Fs),则根据 DLPF_CFG 的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半,如前面所说的,如果设置采样率为 50Hz,那么带宽就应该设置为 25Hz,取近似值 20Hz,就应该设置 DLPF_CFG=100。
4.7 电源管理寄存器2
《MPU6050_Register_Map》对电源管理寄存器2的描述如下所示:
Bit5-Bit0控制加速度和陀螺仪的x/y/z轴是否进入待机模式,本文设置为0,即不进入待机模式。
4.8 陀螺仪数据输出寄存器
《MPU6050_Register_Map》对陀螺仪数据输出寄存器的描述如下所示:
地址0X43和0X44组成陀螺仪X方向角速度,0X45和0X46组成陀螺仪Y方向角速度,0X47和0X48组成陀螺仪Z方向角速度。
4.9 加速度传感器数据输出寄存器
《MPU6050_Register_Map》对加速度传感器数据输出寄存器的描述如下所示:
地址0X3B和0X3C组成X方向加速度,0X3D和0X3E组成Y方向加速度,0X3F和0X40组成Z方向加速度。
4.10 温度数据输出寄存器
《MPU6050_Register_Map》对温度数据输出寄存器的描述如下所示:
地址0X41和0X42组成温度数据,温度换算公式为:
Temperature = 36.53 + regval/340
其中,Temperature 为计算得到的温度值,单位为℃,regval 为从 0X41 和 0X42 读到的温度传感器值。
五、MPU6050 DMP库关键函数介绍
将官方DMP库移植到HARDWARE/MPU6050/eMPL文件夹下,一共6个文件如下图所示:
本文主要介绍2个.c文件:inv_mpu.c 和 inv_mpu_dmp_motion_driver.c。其中我们在inv_mpu.c添加了几个函数,重点是两个函数:mpu_dmp_init和mpu_dmp_get_data。
5.1 mpu_dmp_init
此函数首先通过 IIC_Init(需外部提供)初始化与 MPU6050 连接的 IIC 接口,然后调用mpu_init 函数,初始化 MPU6050,之后就是设置 DMP 所用传感器、FIFO、采样率和加载固件等一系列操作,在所有操作都正常之后,最后通过 mpu_set_dmp_state(1)使能 DMP 功能,在使能成功以后,我们便可以通过 mpu_dmp_get_data 来读取姿态解算后的数据了。具体代码如下所示:
cpp
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_init(void)
{
u8 res=0;
MPU_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;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test(); //自检
if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}else return 10;
return 0;
}
5.2 mpu_dmp_get_data
依据2.2节对于四元数解算欧拉角的原理,本函数用于得到 DMP 姿态解算后的俯仰角、横滚角和航向角。其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数,q30 格式,所以要除以一个 2的 30 次方,其中 q30 是一个常量:1073741824,即 2 的 30 次方,然后带入公式,计算出欧拉角。上述计算公式的 57.3 是弧度转换为角度,即 180/π,这样得到的结果就是以度(°)为单位的。具体代码如下所示:
cpp
//得到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;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
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; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return 2;
return 0;
}
六、程序设计
除了MPU6050官方自带的DMP库,本文还需完成软件I2C实现文件、MPU6050配置文件以及轮询主函数其中软件I2C实现文件详见【STM32开发之寄存器版】(十二)-I2C基础知识详解。
6.1 MPU6050配置文件
该部分文件位于HARDWARE/mpu6050.c,主要包含MPU6050初始化函数、设置陀螺仪和加速度计满量程范围函数、设置数字低通滤波器函数、读取陀螺仪/加速度计/温度计函数等,具体代码如下所示:
cpp
#include "mpu6050.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"
//初始化MPU6050
//返回值:0,成功
// 其他,错误代码
u8 MPU_Init(void)
{
u8 res;
RCC->APB2ENR|=1<<2; //使能PORTA时钟
GPIOA->CRH&=0X0FFFFFFF; //PA15设置成推挽输出
GPIOA->CRH|=0X30000000;
JTAG_Set(SWD_ENABLE); //禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO!!!
MPU_AD0_CTRL=0; //控制MPU6050的AD0脚为低电平,从机地址为:0X68
MPU_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(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;
}
//设置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的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置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为采样率的一半
}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
//得到陀螺仪值(原始值)
//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;;
}
//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
for(i=0;i<len;i++)
{
MPU_IIC_Send_Byte(buf[i]); //发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
while(len)
{
if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK
else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
len--;
buf++;
}
MPU_IIC_Stop(); //产生一个停止条件
return 0;
}
//IIC写一个字节
//reg:寄存器地址
//data:数据
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Byte(u8 reg,u8 data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) //等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(data);//发送数据
if(MPU_IIC_Wait_Ack()) //等待ACK
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Send_Byte(reg); //写寄存器地址
MPU_IIC_Wait_Ack(); //等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令
MPU_IIC_Wait_Ack(); //等待应答
res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
MPU_IIC_Stop(); //产生一个停止条件
return res;
}
6.2 轮询主函数
该部分代码位于USER/test.c,主要功能是读取MPU6050的加速度、角速度、欧拉角以及温度数值,并将其通过串口打印在上位机上。具体代码如下所示:
cpp
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "lcd.h"
#include "key.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
int main(void)
{
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
float temp; //温度
Stm32_Clock_Init(9); //系统时钟设置
uart_init(72,115200); //串口初始化为115200
delay_init(72); //延时初始化
LED_Init(); //初始化与LED连接的硬件接口
KEY_Init(); //初始化按键
MPU_Init(); //初始化MPU6050
while(mpu_dmp_init());
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); //得到陀螺仪数据
printf("Temp = %4.2f, Pitch = %5.2f, Roll = %5.2f, Yaw = %5.2f, ax = %d, ay = %d, az = %d, gx = %d, gy = %d, gz = %d\r\n",temp,pitch,roll,yaw,aacx,aacy,aacz,gyrox,gyroy,gyroz);
}
LED0=!LED0;//LED闪烁
delay_ms(100);
}
}
七、上机实验
将代码烧录至STM32F103ZET6,通过串口上位机XCOM,可以查看得到MPU6050的温度数据、欧拉角数据、加速度数据以及角速度数据。
至此完成本次DMEO!