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ù)首先通過IIC_Init（需外部提供）初始化與MPU6050連接的IIC接口，然后調(diào)用mpu_init函數(shù)，初始化MPU6050，之后就是設(shè)置DMP所用傳感器、FIFO、采樣率和加載固件等一系列操作，在所有操作都正常之后，最后通過mpu_set_dmp_state（1）使能DMP功能，在使能成功以后，我們便可以通過mpu_dmp_get_data來讀取姿態(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)解算后的俯仰角、橫滾角和航向角。不過本函數(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)換成歐拉角。