一、DMP文件的修改：
首先我们打开inv_mpu.c文件夹，如下图所示便是第一个要修改的地方：
我们将其修改为：define定义可以改为自己使用的型号的单片机。
修改后在上面定义这个宏，并加上一个MPU6050的宏，用于源文件区别芯片：
然后我们打开inv_mpu_dmp_motion_driver.c,找到如下地方，和上面一样： 
修改为：（记得在上面加入#define STM32F10x_MPU6050）
此时还有一些地方需要完善，但大致已经改完了，修缮一下：编译，报3处错，如下：
这里报错：（inv_mpu.h文件中）
修改为：
在inv_mpu_dmp_motion_driver.c中：
此时编译还有一个错误：
可以发现是inv_mpu中的问题，我们找到这给注释掉
此时再编译，无错误，一个警告，如下图：
我们点击过去给函数注释掉：（注意一定要注释全，是一整个函数，比较长）
此时再编译：无错误。
二、官方源码的移植
此时，我们的DMP代码已经改完了，下面还要移植官方例子的初始化等代码：下面来转植代码，打开官方库如下文件：
下拉，在main函数复制下面方框的结构体及初始化函数，并自己创建一个函数封装起来并判断返回值：这些函数正确执行就会返回0，否则返回非0数。
到这步你的代码应该是这样：
这几个函数是设置传感器、设置fifo、设置采样率的，复制到自己的代码：
其中DEFAULT_MPU_HZ为官方定义的，ctrl+f搜索一下复制到自己函数上面，然后在相应函数判断返回值，此时代码应该如下：
下面复制DMP初始化步骤代码并粘贴到自己的函数中并判断其返回值：
删掉两行关于注册函数的（未使用到）
再删掉结构体，不使用结构体赋值，如下效果：
这里有个函数报错，去官方库把这个函数复制过来：
即将这两个函数都复制过去并粘贴到MPU6050_DMP_Init上面：
在官方文件找到自检函数复制过来，同样判断返回值：
再将此函数从官方库找出来粘贴到我们初始化函数上方，删掉下图所示内容：
添加返回值，错误返回1，否则为0
自此，其移植已经完成了，下面经行获取姿态数据
三、使用MPU6050获取姿态数据
先写下获取数据的函数，由于DMP会将处理好的数据放入fifo，所以我们只需要读取fifo就可以取出其数据了。至于怎么使用这个函数可以转向定义看看：
定义好相关参数并传到读fifo函数中，再判断其返回值，此时代码应该为：
接下来就开始解析四元数了，将四元数转化为欧拉角：
fifo函数的标志位为sensors，当某个数据存在，读函数会把sensors参数某个位置置一。
我们要获取四元数，判断其标志位，如果有四元数便经行下一步解析。将读出四元数转换成浮点数，直接除2的30次方便将Q30格式换为浮点数了。在上方定义四个浮点型变量用来存储转换后的数据：2^30 = 1073741824,我们宏定义于函数上方，在转换为浮点数，第二张图：
最后将四元数转换为欧拉角，网上百度一下公式，如下：
我们根据其公式编写代码：（乘57.3目的时弧度转换成角度） 至于返回值是好查看错误时是哪步的问题。
此时编译无错误警告，移植完成
需要添加的代码我放下面：
#define DEFAULT_MPU_HZ (100)
//q30格式,long转float时的除数.
#define q30 1073741824.0f
void mget_ms(unsigned long *time)
{
//空函数，未使用
}
static unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
static unsigned short inv_orientation_matrix_to_scalar(
const signed char *mtx)
{
unsigned short scalar;
/*
XYZ 010_001_000 Identity Matrix
XZY 001_010_000
YXZ 010_000_001
YZX 000_010_001
ZXY 001_000_010
ZYX 000_001_010
*/
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
static signed char gyro_orientation[9] = {-1, 0, 0,
0,-1, 0,
0, 0, 1};
static int run_self_test(void)
{
int result;
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x3) {
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
}else return 1;
return 0;
}
int MPU6050_DMP_Init(void)
{
int result;
struct int_param_s int_param;
result = mpu_init(&int_param);//mpu初始化
if(result != 0) return -1;
result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置传感器
if(result != 0) return -2;
result = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置fifo
if(result != 0) return -3;
result = mpu_set_sample_rate(DEFAULT_MPU_HZ);//设置采样率
if(result != 0) return -4;
result = dmp_load_motion_driver_firmware();//加载DMP固件
if(result != 0) return -5;
result = dmp_set_orientation(
inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(result != 0) return -6;
result = 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(result != 0) return -7;
result = dmp_set_fifo_rate(DEFAULT_MPU_HZ);//设置输出速率
if(result != 0) return -8;
result = run_self_test();//自检
if(result != 0) return -9;
result = mpu_set_dmp_state(1);//使能DMP
if(result != 0) return -10;
return result;
}
int mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
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
}else return 2;
return 0;
}我们在主函数就可以读取数据了：
main.c
#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "oled.h"
#include "stdio.h"
#include "mpu6050.h"
#include "inv_mpu.h"
int main(void)
{
u8 string[10] = {0};
float pitch,roll,yaw;
MPU_Init();
MPU6050_DMP_Init();
while (1)
{
mpu_dmp_get_data(&pitch,&roll,&yaw);
sprintf((char *)string,"Pitch:%.2f",1.32);//0300
OLED_ShowString(16,10,string,16,1);
sprintf((char *)string,"Roll :%.2f",roll);//0300
OLED_ShowString(16,26,string,16,1);
sprintf((char *)string,"Yaw :%.2f",yaw);//0300
OLED_ShowString(16,46,string,16,1);
OLED_DrawLine(5,5,125,5,1);
OLED_DrawLine(5,5,5,62,1);
OLED_DrawLine(5,62,125,62,1);
OLED_DrawLine(125,5,125,62,1);
OLED_Refresh();
OLED_Refresh();
if(pitch>70)pitch=70;
if(pitch<-70)pitch=-70;
if(roll>70)pitch=70;
if(roll<-70)pitch=-70;
OLED_Refresh();
}
}上面MPU6050_DMP_Init应该 int型，并返回负数，否则结果不对！
效果：没拍好，但是代码可用
