---

前言

  本笔记记录了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。

  注：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;
};
1
2
3
4
5
6
7

  (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 */
};
1
2
3
4
5
6
7
8
9
10
11
12
13

  (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的结构体
};
1
2
3
4
5
6
7
8
9
10
11
12

  其中flags可以是以下值：

  (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;
};
1
2
3
4
5
6

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

1
2
3
4
5
6
7
8

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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

  这里开始介绍我们可能会用到的重点函数，函数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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

  该函数只需要我们输入总线的名称，如果存在与名称对应的总线，就返回指向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;
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

  这个函数后面的两个传参是一组关系，表示传输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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

  这个函数只是初始化一则消息，然后调用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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

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 

#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 = ®
    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 = ®
    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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404

开个线程运行试试：

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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

结果如下：