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）;

　　//設(shè)置dmp功能

　　if（res）return6;

　　res=dmp_set_fifo_rate（DEFAULT_MPU_HZ）;//設(shè)置DMP輸出速率（最大200Hz）

　　if（res）return7;

　　res=run_self_test（）;//自檢

　　if（res）return8;

　　res=mpu_set_dmp_state（1）;//使能DMP

　　if（res）return9;

　　}

　　return0;

　　}

　　此函數(shù)首先通過(guò)IIC_Init（需外部提供）初始化與MPU6050連接的IIC接口，然后調(diào)用mpu_init函數(shù)，初始化MPU6050，之后就是設(shè)置DMP所用傳感器、FIFO、采樣率和加載固件等一系列操作，在所有操作都正常之后，最后通過(guò)mpu_set_dmp_state（1）使能DMP功能，在使能成功以后，我們便可以通過(guò)mpu_dmp_get_data來(lái)讀取姿態(tài)解算后的數(shù)據(jù)了。

　　mpu_dmp_get_data函數(shù)代碼如下：

　　//得到dmp處理后的數(shù)據(jù)（注意，本函數(shù)需要比較多堆棧，局部變量有點(diǎn)多）

　　//pitch：俯仰角精度：0.1°范圍：-90.0°《---》+90.0°

　　//roll：橫滾角精度：0.1°范圍：-180.0°《---》+180.0°

　　//yaw：航向角精度：0.1°范圍：-180.0°《---》+180.0°

　　//返回值：0，正常

　　//其他，失敗

　　u8mpu_dmp_get_data（float*pitch，float*roll，float*yaw）

　　{

　　floatq0=1.0f，q1=0.0f，q2=0.0f，q3=0.0f;

　　unsignedlongsensor_timestamp;

　　shortgyro［3］，accel［3］，sensors;

　　unsignedcharmore;

　　longquat［4］;

　　if（dmp_read_fifo（gyro，accel，quat，&sensor_timestamp，&sensors，&more））return1;

　　if（sensors&INV_WXYZ_QUAT）

　　{

　　q0=quat［0］/q30;//q30格式轉(zhuǎn)換為浮點(diǎn)數(shù)

　　q1=quat［1］/q30;

　　q2=quat［2］/q30;

　　q3=quat［3］/q30;

　　//計(jì)算得到俯仰角/橫滾角/航向角

　　*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

　　}elsereturn2;

　　return0;

　　}

　　此函數(shù)用于得到DMP姿態(tài)解算后的俯仰角、橫滾角和航向角。不過(guò)本函數(shù)局部變量有點(diǎn)多，大家在使用的時(shí)候，如果死機(jī)，那么請(qǐng)?jiān)O(shè)置堆棧大一點(diǎn)（在startup_stm32f10x_hd.s里面設(shè)置，默認(rèn)是400）。這里就用到了我們前面介紹的四元數(shù)轉(zhuǎn)歐拉角公式，將dmp_read_fifo函數(shù)讀到的q30格式四元數(shù)轉(zhuǎn)換成歐拉角。