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）return6;
res=dmp_set_fifo_rate（DEFAULT_MPU_HZ）;//设置DMP输出速率（最大200Hz）
if（res）return7;
res=run_self_test（）;//自检
if（res）return8;
res=mpu_set_dmp_state（1）;//使能DMP
if（res）return9;
}
return0;
}
此函数首先通过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，正常
//其他，失败
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格式转换为浮点数
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
}elsereturn2;
return0;
}
此函数用于得到DMP姿态解算后的俯仰角、横滚角和航向角。不过本函数局部变量有点多，大家在使用的时候，如果死机，那么请设置堆栈大一点（在startup_stm32f10x_hd.s里面设置，默认是400）。这里就用到了我们前面介绍的四元数转欧拉角公式，将dmp_read_fifo函数读到的q30格式四元数转换成欧拉角。