• 下载方法 

软件下载 |InvenSense 开发人员

​

  • motion driver_6.12 是从invensense网站上下载的最新的DMP库。
  • Motion Driver是传感器驱动程序层的嵌入式软件堆栈，可轻松配置和利用InvenSense运动跟踪解决方案的许多功能。
  • 支持的运动设备为MPU6050 / MPU6500 / MPU9150 / MPU9250。
  • 硬件和板载数字运动处理器（DMP）的许多功能都封装在可以使用和引用的模块化API中。
  • Motion Driver设计为一种可轻松移植到大多数MCU的解决方案。
  • Motion Driver 6.0包括一个用于ARM MCU和TI-MSP430的9轴解决方案。
  • 仅6轴解决方案应继续参考运动驱动程序5.1.2，以便更容易理解软件。

        参考

使用到的文件​

  • inv_mpu.c 实现了与MPU交互的基本功能，如读写寄存器
  • inv_mpu_dmp_motion_driver.c: 实现了与DMP交互的功能，包括：写入DMP固件到MPU、初始化、数据处理。
  • dmpmap.h DMP寄存器映射的定义
  • dmpKey.h DMP 寄存器映射和键值定义
  • inv_mpu.h 头文件
  • inv_mpu_dmp_motion_driver.h: 头文件

文件路径

​

移植 

需要修改的只有inv_mpu.c、inv_mpu.h和inv_mpu_dmp_motion_driver.c，在inv_mpu_dmp_motion_driver.h中添加宏定义

inv_mpu.h

 inv_mpu.c

注意这些宏定义接口函数要符合文件的使用的参数顺序 

 一看就是符合printf()的格式

如果重定义了printf()记得打开

 无论读写顺序都是 设备地址 -->寄存器地址-->字长-->内容或者缓冲区

所以你定义的接口也要符合该规范

比如：

void my_get_ms(unsigned long *time)
{
	*time = HAL_GetTick();
}

 记得在头文件中声明

 inv_mpu_dmp_motion_driver.h

 inv_mpu_dmp_motion_driver.c

移植，其实主要是修改宏定义和添加自己型号得单片机和MPU型号的宏定义，然后剩下就跟着报错修改就行了；

DMP使用以及修改

由图可知DMP对获得到的陀螺仪和加速度计数据，经过处理后输出到FIFO，因此我们直接对FIFO进行读取即可 

在 inv_mpu.c中添加以下代码

代码解析 

1、浮点数的定义

// q30格式,long转float时的除数.
#define q30 1073741824.0f

解释如下

  • 在MPU6050 DMP输出的四元数等数据时，使用的是q30格式，即实际值 = 原始整数值 / 2^30。
  • 这样做的目的是用整数来表示高精度的小数，便于嵌入式系统高效处理，避免浮点运算。
  • 2的30次方（1073741824）是因为用30位来表示小数部分，范围和精度都比较合适。

2、陀螺仪方向设置

// 陀螺仪方向设置
static signed char gyro_orientation[9] = {1, 0, 0,
                                          0, 1, 0,
                                          0, 0, 1};

解释如下：

  • 这是一个3×3的方向矩阵（行优先存储，共9个元素），用于描述陀螺仪（或加速度计）在物理安装方向与DMP（数字运动处理器）内部坐标系之间的映射关系。
  • 当前这个矩阵是单位矩阵（对角线为1，其余为0），表示传感器的XYZ轴与DMP的XYZ轴一一对应，没有旋转或翻转。
  • 如果你的传感器在实际安装时有旋转或翻转，需要修改这个矩阵，使其反映实际的物理方向。

用途：

  • 该矩阵会被传递给 DMP，用于姿态解算时的方向校正。
  • 常用于 dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); 这样的初始化函数中。

知识参考：3维旋转矩阵推导与助记

 3、自检校准

// MPU6050自测试
// 返回值:0,正常
//     其他,失败
uint8_t run_self_test(void)
{
    int result;
    // char test_packet[4] = {0};
    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);
        return 0;
    }
    else
        return 1;
}

  1. 调用自检
    mpu_run_self_test(gyro, accel);
    调用底层自检函数，分别获取陀螺仪和加速度计的自检偏置值，返回值 result 表示自检结果。

  2. 判断自检结果
    if (result == 0x3)
    只有当陀螺仪和加速度计都通过自检（0x3，二进制为11，分别对应陀螺仪和加速度计）时，才继续后续操作，否则返回1表示失败。

  3. 设置陀螺仪偏置

    • 获取陀螺仪灵敏度 mpu_get_gyro_sens(&sens);
    • 将自检得到的偏置值乘以灵敏度，转换为实际单位
    • 调用 dmp_set_gyro_bias(gyro); 设置到 DMP（数字运动处理器）

  4. 设置加速度计偏置

    • 获取加速度计灵敏度 mpu_get_accel_sens(&accel_sens);
    • 将自检得到的偏置值乘以灵敏度
    • 调用 dmp_set_accel_bias(accel); 设置到 DMP

  5. 返回值

    • 成功返回0
    • 失败返回1

总结：
该函数用于在初始化或运行时对 MPU6050 进行自检，确保陀螺仪和加速度计工作正常，并将自检得到的偏置值写入 DMP，用于后续的姿态解算和数据校准。

转到该mpu_run_self_test(gyro, accel)的定义中把下面的这个部分注释掉，不然无法通过后面的判断，无法校准，因为会导致result=0x07

4、方向矩阵转换成方向标量

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;
}
  • 功能：将3x3方向矩阵（9个元素，按行存储）转换为一个16位的方向标量。
  • 实现：
    • inv_row_2_scale(mtx) 处理第一行（X轴），结果放低3位。
    • inv_row_2_scale(mtx + 3) 处理第二行（Y轴），结果左移3位。
    • inv_row_2_scale(mtx + 6) 处理第三行（Z轴），结果左移6位。
    • 最终将三个结果按位或，得到一个唯一的方向标量。
  • 用途：这个标量会传递给DMP，用于告诉DMP当前传感器的物理安装方向
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;
}
  • 功能：将矩阵的一行（3个元素）转换为一个方向编码。
  • 实现：
    • 判断该行哪个元素为正/负，分别对应不同的编码（0~6），7为错误。
    • 例如，[1,0,0] → 0，[0,-1,0] → 5，[0,0,1] → 2。
  • 用途：用于描述每一轴的正负方向。

总结

  • 这两个函数配合使用，将3x3方向矩阵（如gyro_orientation）转换为DMP初始化时需要的方向标量。
  • 这样DMP就能正确理解传感器的物理安装方向，实现姿态解算的正确性。

5、dmp初始化

// mpu6050,dmp初始化
// 返回值:0,正常
//     其他,失败
uint8_t mpu_dmp_init(void)
{
    uint8_t res = 0;
	IIC_Init();
    if (mpu_init() == 0) // 初始化MPU6050
    {
        res = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 设置所需要的传感器
        if (res)
            return 1;
        res = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 设置FIFO
        if (res)
            return 2;
        res = mpu_set_sample_rate(200); // 设置采样率
        if (res)
            return 3;
        res = dmp_load_motion_driver_firmware(); // 加载dmp固件
        if (res)
            return 4;
        res = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪方向
        if (res)
            return 5;
        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)
            return 6;
        res = dmp_set_fifo_rate(200); // 设置DMP输出速率(最大不超过200Hz)
        if (res)
            return 7;
        res = run_self_test(); // 自检
        if (res)
            return 8;
        res = mpu_set_dmp_state(1); // 使能DMP
        if (res)
            return 9;
    }
    else
        return 10;
    return 0;
}

修改部分

 inv_mpu.c

 inv_mpu.h

注意：FIFO的输出速率必须和传感器的采样率一致

  1. DMP_FEATURE_6X_LP_QUAT
    使能6轴低功耗四元数输出（只用陀螺仪和加速度计，适合姿态解算）。

  2. DMP_FEATURE_TAP
    使能敲击（Tap）检测功能。

  3. DMP_FEATURE_ANDROID_ORIENT
    使能Android方向检测（如屏幕方向自动旋转）。

  4. DMP_FEATURE_SEND_RAW_ACCEL
    使能原始加速度计数据输出。

  5. DMP_FEATURE_SEND_CAL_GYRO
    使能校准后的陀螺仪数据输出。

  6. DMP_FEATURE_GYRO_CAL
    使能陀螺仪自动校准功能。

6、四元数转换成欧拉角

【线性代数】矩阵的三重身份_哔哩哔哩_bilibili

// 得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
// pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
// roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
// yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
// 返回值:0,正常
//     其他,失败
uint8_t 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;
    /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
     * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
     **/
    /*if (sensors & INV_XYZ_GYRO )
    send_packet(PACKET_TYPE_GYRO, gyro);
    if (sensors & INV_XYZ_ACCEL)
    send_packet(PACKET_TYPE_ACCEL, accel); */
    /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
     * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
     **/
    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;
}

DMP直接输出的是四元数，所以我们要获取欧拉角要对四元数进行处理，以下是四元数转换成欧拉角的公式

 

自行添加的完整代码

 inv_mpu.h

void my_get_ms(unsigned long *time);
unsigned short inv_row_2_scale(const signed char *row);
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
uint8_t run_self_test(void);
uint8_t mpu_dmp_init(void);
uint8_t mpu_dmp_get_data(float *pitch, float *roll, float *yaw);

 inv_mpu.c


// q30格式,long转float时的除数.
#define q30 1073741824.0f

// 陀螺仪方向设置
static signed char gyro_orientation[9] = {1, 0, 0,
                                          0, 1, 0,
                                          0, 0, 1};
// MPU6050自测试
// 返回值:0,正常
//     其他,失败
uint8_t run_self_test(void)
{
    int result;
    // char test_packet[4] = {0};
    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);
        return 0;
    }
    else
        return 1;
}
// 陀螺仪方向控制
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;
}
// 方向转换
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;
}
// mpu6050,dmp初始化
// 返回值:0,正常
//     其他,失败
uint8_t mpu_dmp_init(void)
{
    uint8_t res = 0;
	IIC_Init();
    if (mpu_init() == 0) // 初始化MPU6050
    {
        res = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 设置所需要的传感器
        if (res)
            return 1;
        res = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 设置FIFO
        if (res)
            return 2;
        res = mpu_set_sample_rate(200); // 设置采样率
        if (res)
            return 3;
        res = dmp_load_motion_driver_firmware(); // 加载dmp固件
        if (res)
            return 4;
        res = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪方向
        if (res)
            return 5;
        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)
            return 6;
        res = dmp_set_fifo_rate(200); // 设置DMP输出速率(最大不超过200Hz)
        if (res)
            return 7;
        res = run_self_test(); // 自检
        if (res)
            return 8;
        res = mpu_set_dmp_state(1); // 使能DMP
        if (res)
            return 9;
    }
    else
        return 10;
    return 0;
}
// 得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
// pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
// roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
// yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
// 返回值:0,正常
//     其他,失败
uint8_t 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;
    /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
     * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
     **/
    /*if (sensors & INV_XYZ_GYRO )
    send_packet(PACKET_TYPE_GYRO, gyro);
    if (sensors & INV_XYZ_ACCEL)
    send_packet(PACKET_TYPE_ACCEL, accel); */
    /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
     * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
     **/
    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;
}

void my_get_ms(unsigned long *time)
{
	*time = HAL_GetTick();
}

# 单片机
Logo
openvela

openvela 操作系统专为 AIoT 领域量身定制，以轻量化、标准兼容、安全性和高度可扩展性为核心特点。openvela 以其卓越的技术优势，已成为众多物联网设备和 AI 硬件的技术首选，涵盖了智能手表、运动手环、智能音箱、耳机、智能家居设备以及机器人等多个领域。

更多推荐

cover

产教共蓉 开源无界：openvela产教生态峰会落地成都，剑指AIoT产业生态与人才双破局

avatar openvela
cover

Sability安卓（四）_ 页面布局开始扫盲，了解古法传统式布局！！！！！

avatar openvela
cover

小米OmniVoice开源600+语种TTS模型，中文准确率吊打ElevenLabs？

avatar openvela