二、 MPU6050 的特点包括:
① 以数字形式输出 6 轴或 9 轴(需外接磁传感器)的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据(需 DMP 支持)
② 具有 131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec的 3 轴角速度感测器(陀螺仪)
③ 集成可程序控制,范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器
④ 移除加速器与陀螺仪轴间敏感度,降低设定给予的影响与感测器的飘移
⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算数据、感测器同步化、姿势感应等的负荷
⑥ 内建运作时间偏差与磁力感测器校正演算技术,免除了客户须另外进行校正的需求
⑦ 自带一个数字温度传感器
⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS
⑨ 可程序控制的中断(interrupt),支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G 中断、零动作感应、触击感应、摇动感应功能
⑩ VDD 供电电压为 2.5V±5%、3.0V±5%、3.3V±5%;VLOGIC 可低至 1.8V± 5%
⑪ 陀螺仪工作电流:5mA,陀螺仪待机电流:5uA;加速器工作电流:500uA,加速器省电模式电流:40uA@10Hz
⑫ 自带 1024 字节 FIFO,有助于降低系统功耗
⑬ 高达 400Khz 的 IIC 通信接口
⑭ 超小封装尺寸:4x4x0.9mm(QFN)
在精英 STM32F1 开发板上,我们通过 PA15 控制 ATK-MPU6050 模块 AD0 接 GND,因而选择 MPU6050 的 IIC 地址是 0X68(不含最低位)
三、接下来,我们介绍一下利用 STM32F1 读取 MPU6050 的加速度和角度传感器数据(非中断方式),需要哪些初始化步骤:
四、接下来我们就可以通过读取相关寄存器,得到加速度传感器,角速度传感器的数据了。下面我们先简单介绍下几种相关寄存器
其中,DEVICE_RESET 位用来控制复位,设置为 1,复位 MPU6050,复位结束后,MPU硬件自动清零该位。SLEEEP 位用于控制 MPU6050 的工作模式,复位后,该位为 1,即进入了睡眠模式(低功耗),所以我们要清零该位,以进入正常工作模式。TEMP_DIS 用于设置是否使能温度传感器,设置为 0,则使能。最后 CLKSEL[2:0]用于选择系统时钟源。
默认是使用内部 8M RC 晶振的,精度不高,所以我们一般选择 X/Y/Z 轴陀螺作为参考的PLL 作为时钟源,一般设CLKSEL=001 即可。
五、DMP 使用简介
我们可以读出 MPU6050 的加速度传感器和角速度传感器的原始数据。不过这些原始数据,对想搞四轴之类的初学者来说,用处不大,我们期望得到的是姿态数据,也就是欧拉角:航向角(yaw)、横滚角(roll)和俯仰角(pitch)。有了这三个角,我们就可以得到当前四轴的姿态,这才是我们想要的结果。要得到欧拉角数据,就得利用我们的原始数据,进行姿态融合解算,这个比较复杂,知识点比较多,初学者 不易掌握。MPU6050 自带了数字运动处理器,即 DMP,并且,InvenSense提供了一个 MPU6050 的嵌入式运动驱动库,结合 MPU6050 的 DMP,可以将我们的原始数据,直接转换成四元数输出,而得到四元数之后,就可以很方便的计算出欧拉角,从而得到 yaw、roll 和 pitch。使用内置的 DMP,大大简化了四轴的代码设计,且 MCU 不用进行姿态解算过程,大大降
低了 MCU 的负担,从而有更多的时间去处理其他事件,提高系统实时性。使用 MPU6050 的 DMP 输出的四元数是 q30 格式的,也就是浮点数放大了 2 的 30 次方倍。在换算成欧拉角之前,必须先将其转换为浮点数,也就是除以 2 的 30 次方,然后再进行计算,计算公式为:
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; //航向角
其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数,q30 格式,所以要除以一个 2的 30 次方,其中 q30 是一个常量:1073741824,即 2 的 30 次方,然后带入公式,计算出欧拉角。上述计算公式的 57.3 是弧度转换为角度,即 180/π,这样得到的结果就是以度(°)为单位的。关于四元数与欧拉角的公式推导,这里我们不进行讲解,感兴趣的朋友,可以自行查阅相关资料学习。
该驱动库,重点就是两个 c 文件:inv_mpu.c 和 inv_mpu_dmp_motion_driver.c。其中我们在inv_mpu.c添加了几个函数,方便我们使用,重点是两个函数mpu_dmp_initmpu_dmp_get_data
这两个函数,这里我们简单介绍下这两个函数。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;
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);
//设置 dmp 功能
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;
}
return 0;
}
此函数首先通过 IIC_Init(需外部提供)初始化与 MPU6050 连接的 IIC 接口,然后调用mpu_init 函数,初始化 MPU6050,之后就是设置 DMP 所用传感器、FIFO、采样率和加载固件等一系列操作,在所有操作都正常之后,最后通过 mpu_set_dmp_state(1)使能 DMP 功能,在使能成功以后,我们便可以通过 mpu_dmp_get_data 来读取姿态解算后的数据了。
mpu_dmp_get_data 函数代码如下:
//得到 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; // 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;
}
此函数用于得到 DMP 姿态解算后的俯仰角、横滚角和航向角。不过本函数局部变量有点多,大家在使用的时候,如果死机,那么请设置堆栈大一点(在 startup_stm32f10x_hd.s 里面设置,默认是 400)。这里就用到了我们前面介绍的四元数转欧拉角公式,将 dmp_read_fifo 函数读到的 q30 格式四元数转换成欧拉角。利用这两个函数,我们就可以读取到姿态解算后的欧拉角,使用非常方便。DMP 部分,我们就介绍到这。