RT_thread的IIC设备学习笔记

---

前言

  本笔记记录了RT_thread系统IIC总线设备的学习，包括简单总结IIC工作原理、API函数的解读，最后使用MPU6050陀螺仪加速度计传感器来应用rtt操作系统封装的IIC设备。

1 IIC简介

  IIC是嵌入式开发中较常见的总线协议，协议包含两条线，一条时钟线和一条数据线，实现半双工双向通信。IIC有从主设备之分，并且允许总线上同时存在多个主设备，但是不同时使用，每个挂载在总线上的设备都有唯一的地址，主设备启动数据传输并产生时钟信号，从设备被主设备寻址，地址匹配上之后就能实现一对主从设备的数据交互了。下面是实际的连接模型。

  IIC的数据传输过程是，假设从设备已经挂载在总线上了，主机先发送开始条件和从设备地址(包含读写位)，从设备接收到自己的地址后产生响应，这一步确认好从设备在线之后主机和从机之间就开始交互数据，每个字节数据正常收发需要有响应来确认，数据传输好了以后主机就发送停止条件。上面的数据传输描述如下图所示：

  Start(开始条件)：SCL为高电平时，主机将SDA拉低，表示数据传输即将开始。
  Slave Addr(从机地址)：7位或者10位器件IIC地址。注意RTThread的I2C设备接口使用的从机地址不包含读写位RW。RW等于0表示写，反之读。
  ACK：当主设备发写信号之后的所有ACK都是从机设备发的；当主设备发送读之后，在数据读取的过程ACK/NACK是由主机设备发的。
  End(停止条件)：在SDA为低电平时，主机将SCL拉高，然后再将SDA拉高，表示传输结束。
  注意，在通信中，主机可能需要和不同的从机设备传输数据或者需要切换读写时(比如你写了某个器件数据，随后想读器件数据，可以省略停止条件发起开始条件)，主机可以重复发送开始条件。

2 RTthread IIC总线设备API

  这一小节记录RTthread在STM32平台下的IIC总线设备驱动，我们可以看到rtthread的IIC总线设备总共有4个源文件，分别是drv_soft_i2c.c、i2c_core.c、i2c_dev.c、i2c_bit_ops.c。

文件	功能简述
drv_soft_i2c.c	封装了I2C对象的初始化函数；IO引脚的读写驱动函数；模拟IIC硬件IO的初始化函数（使用自动初始化机制）
i2c-bit-ops.c	根据IIC时序实现IIC读写接口i2c_bit_xfer(struct rt_i2c_bus_device *bus, struct rt_i2c_msg msgs[], rt_uint32_t num)，设备管理器添加IIC总线设备的接口函数rt_err_t rt_i2c_bit_add_bus(struct rt_i2c_bus_device *bus, const char *bus_name)
i2c_core.c	作为组件，封装了i2c总线设备的注册函数，供i2c-bit-ops.c使用，我们会使用到的函数有（1）rt_i2c_bus_device_find；(2)rt_i2c_master_send;(3)rt_i2c_master_recv
i2c_dev.c	将IIC设备封装成通用的IO设备，可以使用通用的read、write、control访问设备

  注：RTthread的IIC总线设备用的不是硬件IIC，而是模拟IIC。

2.1 IIC设备相关结构体

  (1) 结构体stm32_soft_i2c_config用来保存IIC的配置属性（引脚序号，总线名称）。定义如下：

/* stm32 config class */
struct stm32_soft_i2c_config
{
    rt_uint8_t scl;
    rt_uint8_t sda;
    const char *bus_name;
};

  (2) 结构体struct rt_i2c_bit_ops用来保存IIC的位操作函数，同时也定义了I2C位延时变量和超时时间（这个超时时间是检测时钟线是否在规定时间内置到我们期望的电平，我以前写模拟IIC驱动没有考虑检测时钟线，估计RTthread这么做会比较安全吧，毕竟IO口设置为开漏）。里面的data指针一般会用于指向对象的struct stm32_soft_i2c_config类型变量。

struct rt_i2c_bit_ops
{
    void *data;            /* private data for lowlevel routines */
    void (*set_sda)(void *data, rt_int32_t state);
    void (*set_scl)(void *data, rt_int32_t state);
    rt_int32_t (*get_sda)(void *data);
    rt_int32_t (*get_scl)(void *data);

    void (*udelay)(rt_uint32_t us);

    rt_uint32_t delay_us;  /* scl and sda line delay */
    rt_uint32_t timeout;   /* in tick */
};

  (3) 结构体struct rt_i2c_bus_device，封装了一个I2C总线设备(类，我还是觉得这里说成类好一些)。

/*for i2c bus driver*/
struct rt_i2c_bus_device
{
    struct rt_device parent;//可将IIC封装成rtthread设备
    const struct rt_i2c_bus_device_ops *ops;//I2C总线设备的操作函数
    rt_uint16_t  flags;//
    rt_uint16_t  addr;//存放IIC设备的地址
    struct rt_mutex lock;//通信之前需要获取到互斥量，安全
    rt_uint32_t  timeout;
    rt_uint32_t  retries;
    void *priv;//一般保存的是指向struct rt_i2c_bit_ops的结构体
};

  其中flags可以是以下值：

宏名	对应数值
RT_I2C_WR	0x0000
RT_I2C_RD	(1u << 0)
RT_I2C_ADDR_10BIT	(1u << 2)
RT_I2C_NO_START	(1u << 4)
RT_I2C_IGNORE_NACK	(1u << 5)
RT_I2C_NO_READ_ACK	(1u << 6)
RT_I2C_NO_STOP	(1u << 7)

  (4) 结构体struct stm32_i2c，这个结构体封装了上面(2)、(3)结构体，其定义如下：

/* stm32 i2c dirver class */
struct stm32_i2c
{
    struct rt_i2c_bit_ops ops;
    struct rt_i2c_bus_device i2c2_bus;
};

  (5)结构体struct rt_i2c_msg, 成员列表中的addr是IIC设备地址(不加读写位)，flags取值可以是上面提到的表格，len表示的是buf[]的字节大小。

struct rt_i2c_msg
{
    rt_uint16_t addr;
    rt_uint16_t flags;
    rt_uint16_t len;
    rt_uint8_t  *buf;
};

2.2 rtthread的I2C初始化API

/* I2C initialization function */
int rt_hw_i2c_init(void)//from drv_soft_i2c.c
{
    rt_size_t obj_num = sizeof(i2c_obj) / sizeof(struct stm32_i2c);
    rt_err_t result;

    for (int i = 0; i < obj_num; i++)
    {
        i2c_obj[i].ops = stm32_bit_ops_default;
        i2c_obj[i].ops.data = (void*)&soft_i2c_config[i];
        i2c_obj[i].i2c2_bus.priv = &i2c_obj[i].ops;
        stm32_i2c_gpio_init(&i2c_obj[i]);
        result = rt_i2c_bit_add_bus(&i2c_obj[i].i2c2_bus, soft_i2c_config[i].bus_name);
        RT_ASSERT(result == RT_EOK);
        stm32_i2c_bus_unlock(&soft_i2c_config[i]);

        LOG_D("software simulation %s init done, pin scl: %d, pin sda %d",
        soft_i2c_config[i].bus_name,
        soft_i2c_config[i].scl,
        soft_i2c_config[i].sda);
    }

    return RT_EOK;
}
INIT_BOARD_EXPORT(rt_hw_i2c_init);

  这里开始介绍我们可能会用到的重点函数，函数int rt_hw_i2c_init(void) 初始化系统中涉及到的IIC总线。顺便说下，IIC引脚初始化函数stm32_i2c_gpio_init中是将引脚设置成开漏输出，这样做的好处是不用修改SDA的IO口输入输出方向就能做“双向IO”，不过要注意开漏输出不能少了上拉电阻，不然输出不了高电平。同时需要给IO口写高电平，使引脚处于开漏浮空状态，RTthread的IIC的IO初始化做到了这两点。

2.3 发现IIC总线设备API

  由于我们使用了**int rt_hw_i2c_init(void)**将IIC总线设备注册到rtt设备管理器中了，我们如果想要使用IIC设备的话就需要先找到设备，可以调用下面的API：

struct rt_i2c_bus_device *rt_i2c_bus_device_find(const char *bus_name)
{
    struct rt_i2c_bus_device *bus;
    rt_device_t dev = rt_device_find(bus_name);
    if (dev == RT_NULL || dev->type != RT_Device_Class_I2CBUS)
    {
        LOG_E("I2C bus %s not exist", bus_name);

        return RT_NULL;
    }

    bus = (struct rt_i2c_bus_device *)dev->user_data;

    return bus;
}

  该函数只需要我们输入总线的名称，如果存在与名称对应的总线，就返回指向struct rt_i2c_bus_device类型的句柄，我们可以用这个句柄去读写IIC外设。

2.4 IIC通信API

2.4.1 rt_i2c_transfer

//from i2c_core.c
rt_size_t rt_i2c_transfer(struct rt_i2c_bus_device *bus,
                          struct rt_i2c_msg         msgs[],
                          rt_uint32_t               num)
{
    rt_size_t ret;

    if (bus->ops->master_xfer)
    {
#ifdef RT_I2C_DEBUG
        for (ret = 0; ret < num; ret++)
        {
            LOG_D("msgs[%d] %c, addr=0x%02x, len=%d", ret,
                  (msgs[ret].flags & RT_I2C_RD) ? 'R' : 'W',
                  msgs[ret].addr, msgs[ret].len);
        }
#endif

        rt_mutex_take(&bus->lock, RT_WAITING_FOREVER);
        ret = bus->ops->master_xfer(bus, msgs, num);
        rt_mutex_release(&bus->lock);

        return ret;
    }
    else
    {
        LOG_E("I2C bus operation not supported");

        return 0;
    }
}

  这个函数后面的两个传参是一组关系，表示传输num则消息，函数执行正常会返回传输字节数，使用起来也非常方便，后面会使用它来写陀螺仪加速度计的读写寄存器函数。这里rtt使用该函数衍生出两个API，但是我觉得不灵活，使用少，这里也贴出来看看。

2.4.2 rt_i2c_master_send

//i2c_core.c
rt_size_t master_send(struct rt_i2c_bus_device *bus,
                             rt_uint16_t               addr,
                             rt_uint16_t               flags,
                             const rt_uint8_t         *buf,
                             rt_uint32_t               count)
{
    rt_err_t ret;
    struct rt_i2c_msg msg;

    msg.addr  = addr;
    msg.flags = flags;
    msg.len   = count;
    msg.buf   = (rt_uint8_t *)buf;

    ret = rt_i2c_transfer(bus, &msg, 1);

    return (ret > 0) ? count : ret;
}

  这个函数只是初始化一则消息，然后调用rt_i2c_transfer函数将一则消息发送出去，注意函数rt_i2c_transfer最后会判断flags是否有RT_I2C_NO_STOP，如果没有就会发起IIC停止条件。

2.4.3 rt_i2c_master_recv

//from i2c_core.c
rt_size_t rt_i2c_master_recv(struct rt_i2c_bus_device *bus,
                             rt_uint16_t               addr,
                             rt_uint16_t               flags,
                             rt_uint8_t               *buf,
                             rt_uint32_t               count)
{
    rt_err_t ret;
    struct rt_i2c_msg msg;
    RT_ASSERT(bus != RT_NULL);

    msg.addr   = addr;
    msg.flags  = flags | RT_I2C_RD;
    msg.len    = count;
    msg.buf    = buf;

    ret = rt_i2c_transfer(bus, &msg, 1);

    return (ret > 0) ? count : ret;
}

3 RTT的IIC总线设备实践

  我使用了陀螺仪加速度计MPU6050芯片是实操使用RTT的IIC总线设备，头文件如下：

#ifndef APPLICATIONS_DRV_MPU6050_H_
#define APPLICATIONS_DRV_MPU6050_H_

#define MPU6050G_s250dps            ((float)0.0076335f)  // 0.0087500 dps/LSB
#define MPU6050G_s500dps            ((float)0.0152671f)  // 0.0175000 dps/LSB
#define MPU6050G_s2000dps           ((float)0.0610351f)  // 0.0700000 dps/LSB

enum mpu60xx_set_cmd
{
    MPU6050_PWR_MGMT1,     // power management 1
    MPU6050_PWR_MGMT2,     // power management 2
    MPU6050_GYRO_CONFIG,   // gyroscope configuration(range)
    MPU6050_ACCEL_CONFIG1, // accelerometer configuration(range)
    MPU6050_ACCEL_CONFIG2, // accelerometer configuration2
    MPU6050_INT_ENABLE,    //interrupt enable
    MPU6050_SAMPLE_RATE,
};
typedef enum mpu60xx_set_cmd mpu60xx_set_cmd_t;

enum mpu60xx_gyroscope_range
{
    MPU6050_GYROSCOPE_RANGE0, // ±250dps
    MPU6050_GYROSCOPE_RANGE1, // ±500dps
    MPU6050_GYROSCOPE_RANGE2, // ±1000dps
    MPU6050_GYROSCOPE_RANGE3, // ±2000dps
};
typedef enum mpu60xx_gyroscope_range mpu60xx_gyro_range_t;

enum mpu60xx_accelerometer_range
{
    MPU6050_ACCELEROMETER_RANGE0, // ±2g
    MPU6050_ACCELEROMETER_RANGE1, // ±4g
    MPU6050_ACCELEROMETER_RANGE2, // ±8g
    MPU6050_ACCELEROMETER_RANGE3, // ±16g
};
typedef enum mpu60xx_accelerometer_range mpu50xx_accel_range_t;

typedef struct
{
    struct rt_i2c_bus_device *i2c;//挂载的总线
    rt_mutex_t lock;
    struct offset{
        short int x;
        short int y;
        short int z;
    }accel_offset,gyro_offset;
}MPU6050_Structure;//RTT很喜欢互斥量，我们也使用互斥量

rt_err_t imu_init(const char *i2c_bus_name);

rt_err_t imu_calib_level(unsigned long times);

rt_err_t imu_get_gyro(rt_int16_t *gyro_x, rt_int16_t *gyro_y, rt_int16_t *gyro_z);

rt_err_t imu_get_accel(rt_int16_t *accel_x, rt_int16_t *accel_y, rt_int16_t *accel_z);

rt_err_t MPU60xx_get_param(rt_uint8_t cmd, rt_uint8_t *value);

#endif

Mpu6050驱动源文件：

#include "drv_soft_i2c.h"
#include "drv_mpu6050.h"
#include "string.h"
#include "stdlib.h"

#define DBG_ENABLE
#define DBG_SECTION_NAME "imu"
#define DBG_LEVEL DBG_LOG
#define DBG_COLOR
#include <rtdbg.h>

#define MPU6050_CONFIG_REG        0x1A
#define MPU6050_GYRO_CONFIG_REG   0x1B
#define MPU6050_ACCEL_CONFIG1_REG 0x1C
#define MPU6050_ACCEL_CONFIG2_REG 0x1D
#define MPU6050_INT_ENABLE_REG    0x38
#define MPU6050_ACCEL_MEAS        0x3B
#define MPU6050_GYRO_MEAS         0x43
#define MPU6050_PWR_MGMT1_REG     0x6B
#define MPU6050_PWR_MGMT2_REG     0x6C

#define MPU6050_ADDR 0x68

MPU6050_Structure mpu60xx_device;
static rt_err_t write_reg(rt_uint8_t reg, rt_uint8_t len, rt_uint8_t *buf)
{
    struct rt_i2c_msg msgs[2];
    //设备地址 -- 寄存器号
    msgs[0].addr = MPU6050_ADDR;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf = &reg;
    msgs[0].len = 1;
    //打算往寄存器里写什么
    msgs[1].addr = MPU6050_ADDR;
    msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START;
    msgs[1].buf = buf;
    msgs[1].len = len;

    if(rt_i2c_transfer(mpu60xx_device.i2c, msgs, 2) == 2)
    {
        return RT_EOK;
    }
    else{
        LOG_E("Writing Command Error.");
        return RT_ERROR;
    }
}

static rt_err_t read_reg(rt_uint8_t reg, rt_uint8_t len, rt_uint8_t *buf)
{
    struct rt_i2c_msg msgs[2];

    msgs[0].addr = MPU6050_ADDR;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf = &reg;
    msgs[0].len = 1;

    msgs[1].addr = MPU6050_ADDR;
    msgs[1].flags = RT_I2C_RD;
    msgs[1].buf = buf;
    msgs[1].len = len;

    if(rt_i2c_transfer(mpu60xx_device.i2c, msgs, 2) == 2)
    {
        return RT_EOK;
    }
    else{
        LOG_E("Reading command error.");
        return -RT_ERROR;
    }
}

static rt_err_t reset_imu_device(void)
{
    rt_uint8_t value = 0;
    return write_reg(MPU6050_PWR_MGMT1_REG, 1, &value);
}

static rt_err_t MPU60xx_set_param(rt_uint8_t cmd, rt_uint8_t value)
{
    rt_err_t result = -RT_ERROR;

    switch(cmd)
    {
        case MPU6050_GYRO_CONFIG:
        {
            rt_uint8_t args;

            if(!(value == MPU6050_GYROSCOPE_RANGE0 ||
            value == MPU6050_GYROSCOPE_RANGE1 ||
            value == MPU6050_GYROSCOPE_RANGE2 ||
            value == MPU6050_GYROSCOPE_RANGE3))
            {
                LOG_E("Setting gyroscope range is wrong, please refer gyroscope range");
                return -RT_ERROR;
            }
            result = read_reg(MPU6050_GYRO_CONFIG_REG, 1, &args);
            if(result == RT_EOK)
            {
                args &= 0xE7;
                args |= value << 3;
                result = write_reg(MPU6050_GYRO_CONFIG_REG, 1, &args);
            }
            break;
        }
        case MPU6050_ACCEL_CONFIG1:
        {
            rt_uint8_t args;
            if(!(value == MPU6050_ACCELEROMETER_RANGE0 ||
            value == MPU6050_ACCELEROMETER_RANGE1 ||
            value == MPU6050_ACCELEROMETER_RANGE2 ||
            value == MPU6050_ACCELEROMETER_RANGE3))
            {
                LOG_E("Setting als accelerometer range is wrong, please refer accelerometer range");
                return -RT_ERROR;
            }
            result = read_reg(MPU6050_ACCEL_CONFIG1_REG, 1, &args);
            if(result == RT_EOK)
            {
                args &= 0xE7;
                args |= value << 3;
                result = write_reg(MPU6050_ACCEL_CONFIG1_REG, 1, &args);
            }
            break;
        }
        case MPU6050_SAMPLE_RATE:
        {
            result = write_reg(0x19, 1, &value);
            break;
        }
        case MPU6050_ACCEL_CONFIG2:
        {
            result = write_reg(MPU6050_ACCEL_CONFIG2_REG, 1, &value);
            break;
        }
        case MPU6050_PWR_MGMT1:
        {
            result = write_reg(MPU6050_PWR_MGMT1_REG, 1, &value);
            break;
        }
        case MPU6050_PWR_MGMT2:
        {
            result = write_reg(MPU6050_PWR_MGMT2_REG, 1, &value);
            break;
        }
        case MPU6050_INT_ENABLE:
        {
            result = write_reg(MPU6050_INT_ENABLE_REG, 1, &value);
            break;
        }
        default:
        {
            LOG_E("This cmd '%2x' cant be set or supported", cmd);
            return -RT_ERROR;
        }
    }
    return result;
}

rt_err_t MPU60xx_get_param(rt_uint8_t cmd, rt_uint8_t *value)
{
    rt_err_t result = -RT_ERROR;

    switch(cmd)
    {
        case MPU6050_GYRO_CONFIG:
        {
            rt_uint8_t args;
            result = read_reg(MPU6050_GYRO_CONFIG_REG, 1, &args);
            *value = (args >> 3) & 0x03;
            break;
        }
        case MPU6050_ACCEL_CONFIG1:
        {
            rt_uint8_t args;
            result = read_reg(MPU6050_ACCEL_CONFIG1_REG, 1, &args);
            *value = (args >> 3) & 0x03;
            break;
        }
        case MPU6050_ACCEL_CONFIG2:
        {
            rt_uint8_t args;
            result = read_reg(MPU6050_ACCEL_CONFIG2_REG, 1, &args);
            break;
        }
        case MPU6050_PWR_MGMT2:
        {
            result = read_reg(MPU6050_PWR_MGMT2_REG, 1, value);
            break;
        }
        case MPU6050_INT_ENABLE:
        {
            result = read_reg(MPU6050_INT_ENABLE_REG, 1, value);
            break;
        }
        default:
        {
            LOG_E("This cmd '%2x' cant be get or supported", cmd);
            break;
        }
    }
    return result;
}

rt_err_t imu_get_accel(rt_int16_t *accel_x, rt_int16_t *accel_y, rt_int16_t *accel_z)
{
    rt_err_t result = RT_ERROR;
    rt_uint8_t value[6];

    result = rt_mutex_take(mpu60xx_device.lock, RT_WAITING_FOREVER);
    if(result == RT_EOK)
    {
        result = read_reg(MPU6050_ACCEL_MEAS, 6, &value[0]);
        if(result != RT_EOK)
        {
            LOG_E("Failed to get accelerometer value of imu");
        }
        else{
            *accel_x = (value[0] << 8) + value[1] - mpu60xx_device.accel_offset.x;
            *accel_y = (value[2] << 8) + value[3] - mpu60xx_device.accel_offset.y;
            *accel_z = (value[4] << 8) + value[5] - mpu60xx_device.accel_offset.z;
        }
    }
    else{
        LOG_E("Failed to get accelerometer value of imu");
    }
    rt_mutex_release(mpu60xx_device.lock);
    return result;
}

rt_err_t imu_get_gyro(rt_int16_t *gyro_x, rt_int16_t *gyro_y, rt_int16_t *gyro_z)
{
    rt_err_t result = RT_ERROR;
    rt_uint8_t value[6];

    result = rt_mutex_take(mpu60xx_device.lock, RT_WAITING_FOREVER);
    if(result == RT_EOK)
    {
        result = read_reg(MPU6050_GYRO_MEAS, 6, &value[0]);
        if(result != RT_EOK)
        {
            LOG_E("Failed to get gyroscope value of imu");
        }
        else{
            *gyro_x =  value[0]*256 + value[1] - mpu60xx_device.gyro_offset.x;
            *gyro_y = (value[2] << 8) + value[3] - mpu60xx_device.gyro_offset.y;
            *gyro_z = (value[4] << 8) + value[5] - mpu60xx_device.gyro_offset.z;
        }
    }
    else{
        LOG_E("Failed to get gyroscope value of imu");
    }
    rt_mutex_release(mpu60xx_device.lock);
    return result;
}

rt_err_t imu_calib_level(unsigned long times)
{
    rt_int32_t accel[3] = {0,0,0};
    rt_int32_t gyro[3] = {0,0,0};
    rt_size_t i;
    rt_err_t result = rt_mutex_take(mpu60xx_device.lock, RT_WAITING_FOREVER);
    if(result == RT_EOK)
    {
        for(i=0; i<times; ++i)
        {
            rt_int16_t x,y,z;
            /*read the sensor digital output*/
            result = imu_get_accel(&x, &y, &z);
            if(result == RT_EOK)
            {
                accel[0] += x;
                accel[1] += y;
                accel[2] += z;
            }
            else{
                break;
            }

            result = imu_get_gyro(&x, &y, &z);
            if(result == RT_EOK)
            {
                gyro[0] += x;
                gyro[1] += y;
                gyro[2] += z;
            }
            else{
                break;
            }
        }

        if(result == RT_EOK)
        {
            //获取姿态传感器的三轴加速度原始值
            mpu60xx_device.accel_offset.x = (int16_t)(accel[0] / (int)times);
            mpu60xx_device.accel_offset.y = (int16_t)(accel[1] / (int)times);
            mpu60xx_device.accel_offset.z = (int16_t)(accel[2] / (int)times) - 0xFFF;
            //获取姿态传感器的三轴角速度原始值
            mpu60xx_device.gyro_offset.x = (int16_t)(gyro[0] / (int)times);
            mpu60xx_device.gyro_offset.y = (int16_t)(gyro[1] / (int)times);
            mpu60xx_device.gyro_offset.z = (int16_t)(gyro[2] / (int)times);
        }
    }
    if(result == RT_EOK)
    {
        rt_mutex_release(mpu60xx_device.lock);
    }
    else{
        LOG_E("Can't calibrate the sensor");
    }
    return result;
}

rt_uint8_t MPU60xx_Set_LPF(rt_uint16_t lpf)
{
    rt_uint8_t data = 0;

    if(lpf >= 188)data = 1;

    else if(lpf >= 98)data = 2;

    else if(lpf >= 42)data = 3;

    else if(lpf >= 20)data = 4;

    else if(lpf >= 10)data = 5;

    else data = 6;

    return write_reg(0x1A, 1, &data); //设置数字低通滤波器
}

rt_uint8_t MPU60xx_Set_Rate(rt_uint16_t rate)
{
    rt_uint8_t data;

    if(rate > 1000)rate = 1000;

    if(rate < 4)rate = 4;

    data = 1000 / rate - 1;
    data = write_reg( 0x19, 1, &data);   //设置数字低通滤波器
    return MPU60xx_Set_LPF(rate / 2);   //自动设置LPF为采样率的一半
}

rt_err_t imu_init(const char *i2c_bus_name)
{
    rt_err_t result = -RT_ERROR;

    mpu60xx_device.i2c = rt_i2c_bus_device_find(i2c_bus_name);
    if(mpu60xx_device.i2c == RT_NULL)
    {
        LOG_E("Cant find imu device on '%s'", i2c_bus_name);
        return RT_NULL;
    }
    mpu60xx_device.lock = rt_mutex_create("mutex_imu", RT_IPC_FLAG_FIFO);
    if(mpu60xx_device.lock == RT_NULL)
    {
        LOG_E("Cant create mutex for imu device on '%s'", i2c_bus_name);
        return RT_NULL;
    }

    result = rt_mutex_take(mpu60xx_device.lock, RT_WAITING_FOREVER);
    if(result != RT_EOK)
    {
        goto __exit;
    }

    result = reset_imu_device();
    if(result != RT_EOK)
    {
        goto __exit;
    }
    rt_thread_mdelay(50);
    result = MPU60xx_set_param(MPU6050_PWR_MGMT1, 3);//时钟源选择Z轴
    result = MPU60xx_set_param(MPU6050_PWR_MGMT2, 0);//Open 3 accelerometers and 3 gyroscope
    if(result != RT_EOK)
    {
        goto __exit;
    }
    result = MPU60xx_set_param(MPU6050_GYRO_CONFIG, 3);//Set gyroscope range, default 2000 dps
    if(result != RT_EOK)
    {
        goto __exit;
    }
    result = MPU60xx_set_param(MPU6050_ACCEL_CONFIG1, 2);//Set accelerometer range, default 8g
    if(result != RT_EOK)
    {
        goto __exit;
    }
    result = MPU60xx_set_param(MPU6050_ACCEL_CONFIG2, 1);//ACCEL_FCHOICE_B = 0 and A_DLPF_CFG[2:0] = 1
    if(result != RT_EOK)
    {
        goto __exit;
    }
    MPU60xx_Set_Rate(1000);//1000Hz
__exit:
    if(result != RT_EOK)
    {
        LOG_E("This sensor initializes failure");
    }
    rt_mutex_release(mpu60xx_device.lock);
    return result;
}

开个线程运行试试：

static void imu_thread_entry(void *param)
{
    float delta;
    ivector gyro;
    ivector accel;
    fvector Gy_Dps;

    imu_init("i2c3");
    rt_thread_mdelay(100);
    imu_calib_level(100);
    while(1)
    {
        rt_mutex_take(read_imu_Lock, RT_WAITING_FOREVER);
        imu_get_accel(&accel.x, &accel.y, &accel.z);
        imu_get_gyro(&gyro.x, &gyro.y, &gyro.z);
        Gy_Dps.x = radians(gyro.x * MPU6050G_s2000dps);   // dps
        Gy_Dps.y = radians(gyro.y * MPU6050G_s2000dps);   // dps
        Gy_Dps.z = radians(gyro.z * MPU6050G_s2000dps);   // dps
        delta = Get_DeltaT(GetSysTime_us());
        Quaternion_CF(delta, &accel, &Gy_Dps, &euler);
        rt_mb_send (imu_mb, (rt_uint32_t)&euler);
        rt_mutex_release(read_imu_Lock);
        rt_thread_mdelay(2);
    }
}

int IMU_thread(void)
{
    static rt_thread_t imutask = RT_NULL;
    read_imu_Lock = rt_mutex_create ("Imu_Mutex", RT_IPC_FLAG_FIFO);
    imu_mb = rt_mb_create ("Imu_mb", 1, RT_IPC_FLAG_FIFO);
    imutask = rt_thread_create("IMU_task",
            imu_thread_entry, RT_NULL,
            imu_stack_size, imu_thread_priority, 5);
    if(imutask != RT_NULL)
    {
        rt_thread_startup(imutask);
    }
    return 0;
}
INIT_APP_EXPORT(IMU_thread);

结果如下：