最近查资料发现MPU6500的性能要优于MPU6050，而且目前MPU6050芯片已经停产，最近准备将需要陀螺仪的部分全部替换为MPU6500,所以尝试一下DMP库的姿态解算
代码开源地址:gitee仓库地址

下载DMP库

我在这里使用的是motion_driver6.1 具体的文件可以在仓库里面找到
在这里插入图片描述

下载后是这个样子:
在这里插入图片描述

将里面的.c/.h文件都添加到自己的工程文件中,我这里使用的是Clion进行开发，当然用keil也是一点问题没有的,接下来就可以开始移植代码了

DMP代码移植

官方提供的是基于msp430的工程文件，所以我们要将里面的一些宏定义进行替换为我们自己的代码(比如IIC读写代码,延时代码)\

inv_mpu.h

首先打开inv_mpu.h,在31行的位置有这样的代码
在这里插入图片描述

这段代码主要是为了适应不同开发平台的定义，我们这里不需要，只保留void *arg部分

struct int_param_s {
    void *arg;
};

inv_mpu.c

接着打开inv_mpu.c文件 将27-161行全部删除，这段代码的宏定义主要是陀螺仪型号的选择以及一些函数的实现(IIC读写,延时,调试信息打印).我们将这部分全部替换为自己的代码，具体如下

#define MPU6500//根据自己的陀螺仪信号进行修改，两者的解算代码是不通用的
#include "main.h"
extern I2C_HandleTypeDef hi2c1;  //注意替换为自己的i2c端口,还有下面的iic写入读取函数
#define i2c_write(dev_addr, reg_addr, data_size, p_data) \
HAL_I2C_Mem_Write(&hi2c1, dev_addr, reg_addr, I2C_MEMADD_SIZE_8BIT, p_data, data_size, 0x100)
#define i2c_read(dev_addr, reg_addr, data_size, p_data) \
HAL_I2C_Mem_Read(&hi2c1, dev_addr, reg_addr, I2C_MEMADD_SIZE_8BIT, p_data, data_size, 0x100)
#define delay_ms HAL_Delay
#define get_ms(p) do{ *p = HAL_GetTick();}while(0)
#define log_i(...) do {} while (0)//我们这里不需要打印调试信息
#define log_e(...) do {} while (0)
/* labs is already defined by TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((a<b)?a:b)

利用Ctrl+F打开搜索栏，搜索EMPL_TARGET_STM32F4
在这里插入图片描述

删除ifndef中的代码
接着Ctrl+F搜索hw_s，修改器件地址,一共有两个，分别对应MPU6500和MPU6050所使用的地址，可根据自己的需要修改,将.addr修改为0xd0即可
在这里插入图片描述

在这里插入图片描述

还有一个地方的宏定义我们需要修改,搜索HWST_MAX_PACKET_LENGTH
在这里插入图片描述

将512替换为1024,否则之后自检会出现数组溢出导致程序进入hardfault卡死

inv_mpu_dmp_motion_driver.c

接着我们打开inv_mpu_dmp_motion_driver.c，删除35-76行代码，替换为如下的代码

#include "main.h"
extern I2C_HandleTypeDef hi2c1;
#define delay_ms HAL_Delay
#define get_ms(p) do{ *p = HAL_GetTick();}while(0)
#define log_i(...) do {} while (0)
#define log_e(...) do {} while (0)

然后利用Ctrl+F,搜索_no_operation
在这里插入图片描述

将_no_operation替换为__NOP()😭_no_operation是msp430中使用的NOP是STM32使用的)

验证函数

在代码修改完成后，便可编写初始化代码对DMP库进行调用,这里我参照别的博主的方法。原视频:B站视频地址
但没法直接用在MPU6500上。
在代码的68行左右的自检函数中，需要根据自己的陀螺仪型号选择合适的自检函数
MPU6500使用的是mpu_run_6500_self_test(gyro, accel,0);(0代表不打印调试信息)
MPU6050使用的是mpu_run_self_test(gyro, accel)
需要注意区分
在这里插入图片描述

具体代码如下:
MPU6500.c

#include "MPU6500.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "math.h"

/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from thei
 * driver(s).
 * TODO: The following matrices refer to the configuration on an internal test
 * board at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};

/* These next two functions converts the orientation matrix (see
 * gyro_orientation) to a scalar representation for use by the DMP.
 * NOTE: These functions are borrowed from Invensense's MPL.
 */
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 int run_self_test(void)
{
    int result;
    long gyro[3], accel[3];

    result = mpu_run_6500_self_test(gyro, accel,0);//这里需要根据自己的陀螺仪进行修改
    if (result == 0x07) {
        /* 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 ret;
    struct int_param_s int_param;
    //mpu_init
    ret = mpu_init(&int_param);
    if(ret != 0)
    {
        return ERROR_MPU_INIT;
    }
    //设置传感器
    ret = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    if(ret != 0)
    {
        return ERROR_SET_SENSOR;
    }
    //设置fifo
    ret = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    if(ret != 0)
    {
        return ERROR_CONFIG_FIFO;
    }
    //设置采样率
    ret = mpu_set_sample_rate(DEFAULT_MPU_HZ);
    if(ret != 0)
    {
        return ERROR_SET_RATE;
    }
    //加载DMP固件
    ret = dmp_load_motion_driver_firmware();
    if(ret != 0)
    {
        return ERROR_LOAD_MOTION_DRIVER;
    }
    //设置陀螺仪方向
    ret = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
    if(ret != 0)
    {
        return ERROR_SET_ORIENTATION;
    }
    //设置DMP功能
    ret = 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);
    if(ret != 0)
    {
        return ERROR_ENABLE_FEATURE;
    }
    //设置输出速率
    ret = dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    if(ret != 0)
    {
        return ERROR_SET_FIFO_RATE;
    }
    //自检
    ret = run_self_test();
    if(ret != 0)
    {
        return ERROR_SELF_TEST;
    }
    //使能DMP
    ret = mpu_set_dmp_state(1);
    if(ret != 0)
    {
        return ERROR_DMP_STATE;
    }
    return 0;
}
//DMP解算的结果为四元数，需要进行换算
int MPU6050_DMP_Get_Date(float *pitch, float *roll, float *yaw)
{
    float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
    short gyro[3];
    short accel[3];
    long quat[4];
    unsigned long timestamp;
    short sensors;
    unsigned char more;
    if(dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more))
    {
        return -1;
    }
    if(sensors & INV_WXYZ_QUAT)
    {
        q0 = quat[0] / 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 * (q0 * q3 + q1 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // yaw
    }
    return 0;
}

MPU6500.h

#ifndef INC_MPU6050_H_
#define INC_MPU6050_H_
 
#define ERROR_MPU_INIT      -1 //错误代码所代表的含义
#define ERROR_SET_SENSOR    -2
#define ERROR_CONFIG_FIFO   -3
#define ERROR_SET_RATE      -4
#define ERROR_LOAD_MOTION_DRIVER    -5
#define ERROR_SET_ORIENTATION       -6
#define ERROR_ENABLE_FEATURE        -7
#define ERROR_SET_FIFO_RATE         -8
#define ERROR_SELF_TEST             -9
#define ERROR_DMP_STATE             -10
 
#define DEFAULT_MPU_HZ  100
#define Q30  1073741824.0f
 
int MPU6050_DMP_init(void);//DMP初始化函数
int MPU6050_DMP_Get_Date(float *pitch, float *roll, float *yaw);//DMP调用函数
 
#endif /* INC_MPU6050_H_ */

主函数验证

调用函数写完后，我们便可以在主函数中进行验证,我在这里使用OLED打印解算结果(使用CLion的记得打开浮点数打印支持,仓库里有具体说明)
我这里根据自己的OLED代码进行调用，放在这里作为参考，可根据自己的喜好替换为串口打印或别的方式

/* USER CODE BEGIN 2 */
  HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
  OLED_Init();
  OLED_Clear();
  int ret=0;
  do {
    ret=MPU6050_DMP_init();//DMP初始化
  } while (ret);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    MPU6050_DMP_Get_Date(&pitch,&roll,&yaw);//DMP数据获取
    sprintf(String,"pitch:%.1f",pitch);
    OLED_ShowString(0,0,String,8);
    sprintf(String,"roll:%.1f",roll);
    OLED_ShowString(0,16,String,8);
    sprintf(String,"yaw:%.1f",yaw);
    OLED_ShowString(0,32,String,8);
    OLED_Update();
  }
  /* USER CODE END 3 */
Date(&pitch,&roll,&yaw);//DMP数据获取
    sprintf(String,"pitch:%.1f",pitch);
    OLED_ShowString(0,0,String,8);
    sprintf(String,"roll:%.1f",roll);
    OLED_ShowString(0,16,String,8);
    sprintf(String,"yaw:%.1f",yaw);
    OLED_ShowString(0,32,String,8);
    OLED_Update();
  }
  /* USER CODE END 3 */

实验现象

接线

我所有的IIC设备都挂在hi2c1下面直接串接即可
3V3–3V3
PB6–SCL
PB7–SDA
GND–GND

现象

在这里插入图片描述

# stm32 # 嵌入式硬件 # 单片机
Logo
AI硬件创业社区

智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。

更多推荐

cover

工业网关的Modbus协议优化：为什么你的心跳包总丢帧？

avatar AI硬件创业社区
cover

电容品牌差一档，语音电源噪声飙升：BOM降本中的ESR陷阱与实测复现

avatar AI硬件创业社区
cover

边缘AI为何总卡在TensorRT部署？实测HDR传感器与噪声模型的隐藏陷阱

avatar AI硬件创业社区