STM32 HAL库移植MPU6050 DMP库(跟着做就能成功)

STM32移植MPU6050 DMP库(跟着做就能成功)

  • 0.前言
  • 一、使用CubeMX建立工程
  • 二、移植DMP库
    • 修改:
      • 注意:
    • 使用
  • 三、总结


0.前言

  最近捣鼓FreeRTOS时需要使用一个MPU6050姿态传感器,到处扒拉例程时发现各式各样的教程都不太满意,感觉差点意思,所以自己慢慢摸索整了一个移植教程,有疏漏的地方还请各位读者进行指正。

一、使用CubeMX建立工程

  有关的理论部分这里就不进行赘述了,此篇文章目的是实现STM32F103RCT6驱动MPU6050传感器,并通过串口发送解析完成的数据,在I2C1总线上挂载MPU6050外设和OLED屏幕,实时读取并显示数据。
以下是CubeMX的配置:
使能I2C1:
在这里插入图片描述
使能UART1串口:
在这里插入图片描述
时钟及其他外设请按需使能,这里笔者额外添加了FreeRTOS的CMSIS_V2版本,实际并未使用相关组件,可忽略。

二、移植DMP库

MotionDriver_V6.1下载:https://os.mbed.com/users/oprospero/code/MotionDriver_6_1
下载后会得到一个基于MSP430的工程,主要需要移植以下文件:
在这里插入图片描述
需要将红框中的文件添加进工程,motion_driver_test.c为测试程序,不需要添加,不过可以仿照其编写自己的测试程序。

修改:

在官方的MSP430相关的代码基础上进行修改,首先在inv_mpu.h中添加所需要的宏,并排除掉影响不大的报错:
在这里插入图片描述
其中STM32_MPU6050为后续需要使用的自定义宏,MPU6050为DMP库中指定的陀螺仪型号,下方的结构体没有使用到,添加此参数排除报错。

在inv_mpu.c中修改原本的宏定义,并实现自己的相关函数:
在这里插入图片描述
在这里插入图片描述

如果未使用FreeRTOS则将毫秒延时定义为HAL_Delay()函数,并去除此文件中的reg_int_cb()部分(未使用)。

同样在inv_mpu_dmp_motion_driver.c文件中也修改为自己的操作函数,并将__no_operation()函数改为__NOP():
在这里插入图片描述
在这里插入图片描述
至此,DMP的初步移植工作已完成,能够编译通过。

注意:

经过笔者实测,如果想要成功读取传感器数据,还需要把inv_mpu.c中的MPU6050设备地址修改为0xD0,否则会找不到设备。
在这里插入图片描述

使用

参考motion_driver_test.c文件中的相关流程,新建my_mpu6050.c和my_mpu6050.h文件,实现MPU初始化和数据获取。
my_mpu6050.c:

#include "my_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;
}

my_mpu6050.h:

#ifndef __MY_MPU6050_H__
#define __MY_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

然后在主函数中对数据进行获取即可:
在这里插入图片描述
到此即移植完成。
在这里插入图片描述

三、总结

  本文中还存在一些优化空间,例如没有使用DMA进行数据传输,笔者在使用DMA时出现I2C总线读写超时的问题,经过多方查找也没有解决,猜测是每次读写的间隔太小导致,有一种办法是调用HAL_I2C_Mem_Transfer_DMA()后使用while()阻塞等待传输完成信号,不过这样就和直接使用I2C没有区别了,所以最后也没有使用。除此之外也没有对偏航角进行补偿,有需求的还需要自己花时间研究。
  后续打算使用拓展卡尔曼进行姿态解算,所以本篇暂时先做到这里。

  • 35
    点赞
  • 109
    收藏
    觉得还不错? 一键收藏
  • 20
    评论
评论 20
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值