当前位置：物联沃-IOTWORD物联网 > 技术教程 > 使用STM32 HAL库通过I2C读取MPU6050传感器数据 DMP motion_driver 5.1.3版本
代码收藏家代码收藏家 技术教程

使用STM32 HAL库通过I2C读取MPU6050传感器数据 DMP motion_driver 5.1.3版本

背景

简单记录下STM32与MPU6050通信实现中遇到的一些坑。目前网上能查到的资料基本都是软件模拟I2C，并且DMP driver的版本较旧。我这里直接使用Cube IDE 生成的HAL库I2C通信，DMP motion driver 使用5.1.3版本。

开发环境

  • Ubuntu 20.04 LTS
  • STM32CubeIDE 1.9.0
  • MPU6050 DMP 驱动目前最新版本  motion_driver 5.1.3 Software Downloads | InvenSense Developers
  • 芯片STM32 F103C8T6

    • 移植motion_driver 5.1.3

    移植过程主要参考了B站这个视频 STM32移植MPU6050DMP库1_哔哩哔哩_bilibili 感谢UP主 一键三连这次一定。

    复制 motion_driver-5.1.3/core/driver/eMPL/下的6个文件

  • dmpKey.h
  • inv_mpu.c  
  • inv_mpu_dmp_motion_driver.h
  • dmpmap.h  
  • inv_mpu_dmp_motion_driver.c  
  • inv_mpu.h

    • 修改inv_mpu.h 和 inv_mpu.c

    1. 在inv_mpu.h新增STM32设备的宏定义

    #define STM32_MPU6050 //新增32设备宏定义 在.c中改写原msp430的宏
#define MPU6050 //必须这样命名 用于驱动库文件中开启MPU6050

    2. 在inv_mpu.c 改写msp430的宏，改为头文件新增的STM32_MPU6050。去掉msp430相关头文件及结构体。

  • 新增包含HAL库的i2c.h
  • 用HAL库的I2C读写函数HAL_I2C_Mem_Write和HAL_I2C_Mem_Read改写原 i2c_write和i2c_read宏函数。这里需要注意，inv_mpu.c 第491行，对MPU6050地址的定义的是0x68，i2c需要高七位地址，改写读写函数时需要左移1位。如果MPU6050的AD0引脚接VDD时，需要修改成0x69
    • #if defined STM32_MPU6050 //新增32设备宏定义

// 去掉MSP430的头文件
//#include "msp430.h"
//#include "msp430_i2c.h"
//#include "msp430_clock.h"
//#include "msp430_interrupt.h"

// 用HAL库重写宏
#define i2c_write(mpuaddr, regaddr, dsize, pdata)	HAL_I2C_Mem_Write(&hi2c1, mpuaddr << 1, regaddr, I2C_MEMADD_SIZE_8BIT, pdata, dsize, HAL_MAX_DELAY)
#define i2c_read(mpuaddr, regaddr, dsize, pdata)    HAL_I2C_Mem_Read(&hi2c1, mpuaddr << 1, regaddr, I2C_MEMADD_SIZE_8BIT, pdata, dsize, HAL_MAX_DELAY)
#define delay_ms(ms)    HAL_Delay(ms)
#define get_ms(p)      do { *p = HAL_GetTick();} while(0)

// 去掉MSP430相关结构体
//static inline int reg_int_cb(struct int_param_s *int_param)
//{
//    return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
//        int_param->active_low);
//}
#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)

    写用于初始化及读取数据的MPU6050.h MPU6050.c

    1. 初始化函数 int MPU6050_Init(void);

    参照官方例程 motion_driver-5.1.3/simple_apps/msp430/ motion_driver_test.c 写初始化函数 int MPU6050_Init(void)

    int MPU6050_Init(void)
{
	int result;
	struct int_param_s int_param;
	struct hal_s hal = {0};

	HAL_Delay(100); // 一定要加延时！！
	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;

	hal.dmp_features = 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;
    result = dmp_enable_feature(hal.dmp_features); // 设置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 0;

}
  • 特别注意，我一直遇到 mpu_init 初始化失败的情况。在 mpu_init 之前加入100毫秒延时后成功初始化。大概是MPU上电后需要等待一定时间
  • 从 inv_mpu.h 复制结构体 int_param_s，用于初始化函数 mpu_init
  • 为采样率设置函数 mpu_set_sample_rate 定义 #define DEFAULT_MPU_HZ  (100) 其频率可设置区间为4Hz 至 1kHz
  • 为方向设置函数 dmp_set_orientation 从官方例程 motion_driver_test.c 中复制函数inv_row_2_scale 和 inv_orientation_matrix_to_scalar 及数组 gyro_orientation[9]
  • 增加自检函数，从官方例程 motion_driver_test.c 中复制函数 run_self_test ，特别注意UP主的视频及网上的资料基本使用的是老的driverv版本，在12行判断result处，老版本需要改为0x03，但在motion_driver 5.1.3中，在 inv_mpu.c 的2747行我们可以看到默认 result |= 0x04; 这样结果还是0x07，不需要修改官方例程的自检函数了
    • static int run_self_test(void)
{
    int result;
    long gyro[3], accel[3];
    unsigned char i = 0;

#if defined (MPU6500) || defined (MPU9250)
    result = mpu_run_6500_self_test(gyro, accel, 0);
#elif defined (MPU6050) || defined (MPU9150)
    result = mpu_run_self_test(gyro, accel);
#endif
    if (result == 0x07) //判断返回值 新版默认|0x04，不需要由0x07修改至0x03
    {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        for(i = 0; i<3; i++)
        {
        	gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps
        	accel[i] *= 2048.f; //convert to +-16G
        	accel[i] = accel[i] >> 16;
        	gyro[i] = (long)(gyro[i] >> 16);
        }

        mpu_set_gyro_bias_reg(gyro);

#if defined (MPU6500) || defined (MPU9250)
        mpu_set_accel_bias_6500_reg(accel);
#elif defined (MPU6050) || defined (MPU9150)
        mpu_set_accel_bias_6050_reg(accel);
#endif
    }
    else
    {
    	return -1;
    }

    return 0;
}
    2. 读数据函数int MPU6050_DMP_GetData

    通过inv_mpu_dmp_motion_driver.c 的 dmp_read_fifo (gyro, accel, quat, timestamp, sensors, more)函数读取四元数数据。相应的参数：

    1. gyro 为陀螺仪数据，需传入三个 short 型元素的数组
    2. accel 加速度数据，需传入三个 short 型元素的数组
    3. quat 四元数数据，需传入四个 long 型元素的数组
    4. timestamp 毫秒时间戳 usigned long 型
    5. sensors 标志位，short型
    6. more unsigned char 型，返回FIFO中剩余的数据包个数
    int MPU6050_DMP_GetData(long *quat, double *pitch, double *roll, double *yaw)
{

	short gyro[3];
	short accel[3];
	double q[3] = {0.0f};
	unsigned long timestamp;
	short sensors;
	unsigned char more;

	while (dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more));


	if (sensors & INV_WXYZ_QUAT)
	{
	    for (short i = 0; i < 4; i++)
	        q[i] = quat[i] / Q30;

     *pitch = asin(-2 * q[1] * q[3] + 2 * q[0] * q[2]) * 57.3;
     *roll  = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2] * q[2] + 1) * 57.3;
     *yaw   = atan2(2 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) * 57.3;
	}



	return 0;
}
  • 使用dmp_read_fifo函数读取数据，进行判断，其值不为0时，读取成功，失败返回-1。特别注意这里常出现的错误是FIFO溢出，读不到数据。主要是由于使用延迟函数和DMP速率设置等问题导致的。UP主的视频中使用一次 if 判断，我调试时经常会卡在这里。进入dmp_read_fifo 我们可以看到在 mpu_read_fifo_stream 函数中，失败会调用 mpu_reset_fifo() 重置 fifo，所以这里我使用while循环，直到读取成功后向后进行
  • 判断sensors & INV_WXYZ_QUAT == 1时 即得到 quat 数据，根据需要，这里可以将四元数转换成欧拉角，公式如程序所列。四元数数据是Q30格式的，使用欧拉角公式时候，需转成浮点型数据。特别注意，UP主及网上大部分的代码,使用的是 flot 型存储欧拉角，但是我的环境下 flot 会发生溢出导致欧拉角数据都是0，这里应使用 double 型

    • 结语

    整个调试的过程比较曲折，文中标黄的特别注意都是我踩坑的地方，特写此文记录下，也希望能帮到更多的人。目前还没有解决的问题是使用I2C中断通讯，会导致mpu_init 初始化失败，还在尝试。

    作者：冷锋卉

    物联沃分享整理
    物联沃-IOTWORD物联网 » 使用STM32 HAL库通过I2C读取MPU6050传感器数据 DMP motion_driver 5.1.3版本

    代码收藏家 普通

    分享到：

    相关推荐

    发表回复