这里,我们主要关心数字低通滤波器(DLPF)的设置位,即:DLPF_CFG[2:0],加速度计和陀螺仪,都是根据这三个位的配置进行过滤的。DLPF_CFG不同配置对应的过滤情况如表1.1.2所示:
图1.1.2DLPF_CFG配置表
这里的加速度传感器,输出速率(Fs)固定是1Khz,而角速度传感器的输出速率(Fs),则根据DLPF_CFG的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半,如前面所说的,如果设置采样率为50Hz,那么带宽就应该设置为25Hz,取近似值20Hz,就应该设置DLPF_CFG=100。
最后,温度传感器的值,可以通过读取0X41(高8位)和0X42(低8位)寄存器得到,温度换算公式为:
Temperature=36.53+regval/340
其中,Temperature为计算得到的温度值,单位为℃,regval为从0X41和0X42读到的温度传感器值。
2、DMP使用简介
使用内置的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/π,这样得到的结果就是以(°)为单位的。
其中我们在inv_mpu.c添加了几个函数,方便我们使用,重点是两个函数:mpu_dmp_init和mpu_dmp_get_data这两个函数,这里我们简单介绍下这两个函数。
mpu_dmp_init,是MPU6050DMP初始化函数,该函数代码如下:
//mpu6050,dmp初始化
//返回值:0,正常
//其他,失败
u8mpu_dmp_init(void)
{
u8res=0;
IIC_Init();//初始化IIC总线
if(mpu_init()==0)//初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//需要的传感器
if(res)return1;
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
if(res)return2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ);//设置采样率
if(res)return3;
res=dmp_load_motion_driver_firmware();//加载dmp固件
if(res)return4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
//设置陀螺仪方向
if(res)return5;