MPU6050篇——姿态解算，卡尔曼滤波_mpu6050姿态解算代码

一、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型，并返回负数，否则结果不对！

效果：没拍好，但是代码可用