這里,我們主要關心數字低通濾波器(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;