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 20.04.2 LTS + ROS-noetic

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

创建功能包：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
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <sstream>
#include "MPU9250_I2C_pkg/mpu9250.h"



int main(int argc,char **argv)
{
    int16_t accelgyrodata[6] = {0};

    MPU9250 mpu9250;
    ros::init(argc,argv,"Imupublisher");
    ros::NodeHandle seuNB;
    mpu9250.Init();

    ros::Publisher tree = seuNB.advertise<sensor_msgs::Imu>("MPU9250_Topic",10);   

    sensor_msgs::Imu imu_msg;

    ros::Rate loop_rate(1);
    while(ros::ok())
    {
        mpu9250.MPU9250_Accel_Gyro_Reda(accelgyrodata);

    imu_msg.header.stamp = ros::Time::now();
    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;



         tree.publish(imu_msg);
            loop_rate.sleep();
    }

}

工程概述

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

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

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