stm32f103 MPU6050利用DMP进行姿态解算(硬件iic,hal库)

​

最近在捣鼓平衡车，需要用到MPU6050进行姿态解算，参考了一些文章，也自己解决了一些问题

参考:2_小学生都能搞定的MPU6050DMP库向STM32HAL库的移植_哔哩哔哩_bilibili

由于在移植的时候用的DMP是官网下载,所以和视频的讲解有些许不同的地方，如果所有资料都是按照视频中来的话，理论上是不需要额外的改动的.视频中自定义的MPU6050代码也会放在文章后面:

本文主要讲解如何移植DMP库，cubemx请自行配置

MotionDriver_V6.1下载：https://os.mbed.com/users/oprospero/code/MotionDriver_6_1
下载后会得到一个基于MSP430的工程，主要需要移植以下文件：

需要将红框中的文件添加进工程，motion_driver_test.c为测试程序，不需要添加，不过可以仿照其编写自己的测试程序。
首先是inv_mpu.h大概31行的位置

 只保留void *arg

struct int_param_s {
//#if defined EMPL_TARGET_MSP430 || defined MOTION_DRIVER_TARGET_MSP430
//    void (*cb)(void);
//    unsigned short pin;
//    unsigned char lp_exit;
//    unsigned char active_low;
//#elif defined EMPL_TARGET_UC3L0
//    unsigned long pin;
//    void (*cb)(volatile void*);
    void *arg;
//#elif defined EMPL_TARGET_STM32F4
//    void (*cb)(void);
//#endif
};

接着打开inv_mpu.h

将27-161行代码全部删除,替换如下: (本文的行数都是修改前原文件的行数)

#define MPU6050
#include "main.h"
extern I2C_HandleTypeDef hi2c2;  //注意替换为自己的i2c端口,还有下面的iic写入读取函数
#define i2c_write(dev_addr, reg_addr, data_size, p_data) \
HAL_I2C_Mem_Write(&hi2c2, 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(&hi2c2, 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)

在大约774行的地方 ,删除ifndef中的代码

接着利用ctrl+f搜索hw,改一下器件地址

有两个，保险起见还是都改掉把 

 接着打开inv_mpu_dmp_motion_driver.c,删除35-76行代码

替换为下面的代码

#include "main.h"
extern I2C_HandleTypeDef hi2c2;
#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)

 然后再大概635行的位置

__no_operation修改为__NOP();

原视频到这里就开始在主函数里面验证了，但是官网下载的文件还是不能正常运行，在我调试的时候发现卡在MPU6050.c自检函数这个位置 

(就是这里result为0x7而不是0x3)

经过我的调试发现，问题出现在inv.mpu.c2618行这个位置(可以直接右键跳转到这个位置),2609处的两个if函数之后result为0x3,但是又在下面或了0x04,变为0x7，把这部分注释掉即可 

最后就是我们的主函数验证,

这里我使用串口发送数据来验证，也可以利用oled验证 

视频中所用的MPU6050文件(均来自b站up,使用时记得修改):

MPU6050.c:

/*
 * MPU6050.c
 *
 *  Created on: Aug 25, 2024
 *      Author: 王滋行
 */

#include "MPU6050.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_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 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;
}

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;
}

MPU6050.h

/*
 * MPU6050.h
 *
 *  Created on: Aug 25, 2024
 *      Author: 王滋行
 */

#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);
int MPU6050_DMP_Get_Date(float *pitch, float *roll, float *yaw);

#endif /* INC_MPU6050_H_ */

​