PS：本文需要的先行知识请自行补充

本文用linux的I2c_dev，去对I2C设备进行操作(open/read/write/close)，实现部署于ROS中，并结合ROS特性完成于MPU9250的通讯

linux-I2C

代码需包含的头文件：

1
2
3
4
5
6
7
8
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>

开启I2C并进行初始化：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
fd = open(I2C_DEV,O_RDWR);
if(fd < 0)
{
    // printf("open %s failed\n",I2C_DEV);
    std::stringstream ss;
    ss << "Failed to open " << I2C_DEV << " device";
    throw std::runtime_error(ss.str());
}
int ret = ioctl(fd,I2C_SLAVE,Device_Address);
if(ret < 0)
{
    // printf("setenv address faile ret: %x \n", ret);
    throw std::runtime_error("Failed to set I2C address");  
}

读写操作：

1
2
3
ssize_t bytesRead ;
 bytesRead = write(fd,addr,1);
 bytesRead = read(fd,buf,6);

ROS实现I2C通讯 MPU9250

工程环境：RK3588 + Ubuntu 22.04.2 LTS + ROS2-humble

默认工作空间已建立，环境变量已导入脚本；

创建功能包：catkin_create_pkg myi2C_MPU9250_pkg roscpp rospy std_msgs

MPU9250 BSP代码设计

部分宏定义：

1
2
3
4
#define I2C_DEV "/dev/i2c-6"
#define Device_Address 0x68
#define gravity 9.8

定义MPU9250类：

1
2
3
4
5
6
7
8
9
10
11
12
13
class MPU9250
{
    public:
        MPU9250();

        void Init();
        int8_t MPU9250_SetLPF(int16_t LPF);
        void MPU9250_Accel_Gyro_Reda(int16_t *AccelGyroData);
        void MPU9250_Temp_Read(int16_t *TempData);
        void MPU9250_Set_ZeroBias();
    private:
        int fd;
};

部分函数封装：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
MPU9250::MPU9250()
{
    fd = open(I2C_DEV,O_RDWR);
    if(fd < 0)
    {
        // printf("open %s failed\n",I2C_DEV);
        std::stringstream ss;
        ss << "Failed to open " << I2C_DEV << " device";
        throw std::runtime_error(ss.str());
    }
    int ret = ioctl(fd,I2C_SLAVE,Device_Address);
    if(ret < 0)
    {
        // printf("setenv address faile ret: %x \n", ret);
        throw std::runtime_error("Failed to set I2C address");  
    }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
void MPU9250::MPU9250_Accel_Gyro_Reda(int16_t *AccelGyroData)
{
    char addr[1];
    char buf[6];
    ssize_t bytesRead ;


    addr[0] = MPU_ACCEL_XOUTH_REG;
    bytesRead = write(fd,addr,1);
    bytesRead = read(fd,buf,6);

    AccelGyroData[0] = (int16_t)((buf[0]<<8)|buf[1]);
    AccelGyroData[1] = (int16_t)((buf[2]<<8)|buf[3]);
    AccelGyroData[2] = (int16_t)((buf[4]<<8)|buf[5]);

    addr[0] = MPU_GYRO_XOUTH_REG;
    bytesRead = write(fd,addr,1);
    bytesRead = read(fd,buf,6);

    AccelGyroData[3] = (int16_t)((buf[0]<<8)|buf[1]);
    AccelGyroData[4] = (int16_t)((buf[2]<<8)|buf[3]);
    AccelGyroData[5] = (int16_t)((buf[4]<<8)|buf[5]);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
void MPU9250::MPU9250_Temp_Read(int16_t *TempData)
{
    char addr[1];
    char buf[6];
    short data;
    ssize_t bytesRead ;

    addr[0] = MPU_TEMP_OUTH_REG;
    bytesRead = write(fd,addr,1);
    bytesRead = read(fd,buf,6);   
    data = (int16_t)((buf[0] << 8) | buf[1]);
   *TempData = 21.0 + ((float)data / 333.87f);        
}

ROS节点设计

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
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "mpu9250.h"

using namespace std::chrono_literals;

class ImuPublisher : public rclcpp::Node
{
    public:
        using ImuMsg = sensor_msgs::msg::Imu;
        ImuPublisher()
        : Node("imu_publisher")
        {
            mpu9250.Init();

            imu_publisher_ = this->create_publisher<ImuMsg>("imu/data_raw", 3);

            timer_ = this->create_wall_timer(1ms, std::bind(&ImuPublisher::timer_callback, this));
        }
    private:

    void timer_callback()
    {
        ImuMsg imu_msg;
        int16_t accelgyrodata[6] = {0};

        rclcpp::Time Stamp = rclcpp::Node::now();

        mpu9250.MPU9250_Accel_Gyro_Reda(accelgyrodata);

        imu_msg.header.stamp = Stamp;
        imu_msg.header.frame_id = "Imu_frame";

        imu_msg.angular_velocity.x = accelgyrodata[3] / 16.4;
        imu_msg.angular_velocity.y = accelgyrodata[4] / 16.4;
        imu_msg.angular_velocity.z = accelgyrodata[5] / 16.4;

        imu_msg.linear_acceleration.x = accelgyrodata[0] / 8192.0 * gravity;
        imu_msg.linear_acceleration.y = accelgyrodata[1] / 8192.0 * gravity;
        imu_msg.linear_acceleration.z = accelgyrodata[2] / 8192.0 * gravity;

        imu_publisher_->publish(imu_msg);
    }
    MPU9250 mpu9250;

    rclcpp::Publisher<ImuMsg>::SharedPtr imu_publisher_;

    rclcpp::TimerBase::SharedPtr timer_;

};


int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ImuPublisher>());

  rclcpp::shutdown();
  return 0;
}

工程概述

在/dev/ 中可以查询设备的/dev/i2c-* ，选择需要的I2C即可

本例程中使用/dev/i2c-6

功能：
ROS2从MPU9250传感器读取加速度计和陀螺仪数据，并以IMU消息的格式通过话题发布这些数据