乐视加装ROS-IMU变身Astra520I_icm20948 linux 驱动-优快云博客

本文链接：https://blog.youkuaiyun.com/weixin_45867032/article/details/109680723

本文介绍如何将乐视设备加装ROS-IMU模块并实现250Hz频率的IMU数据发布，包括MPU9250的DMP移植、STM32编程配置及IIC运行等内容。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

乐视加装ROS—IMU变身Astra520I

说明

本品可通过ROS读取IMU下位机，发布IMU话题（包含加速度计和陀螺仪），频率可达250HZ

MPU9250权威参考

MPU9250 使用 DMP 输出姿态角：DMP 库到 STM32 平台移植笔记

IMU消息定义

在这里插入图片描述

某宝ROSIMU链接
传感器：ICM20948
Github–ICM20689 DMP Driver
自检初始化参考
 MPU9250和ICM20948对比
 6轴官网

编程

时间戳

时间戳标定

D435i时间戳标定示例

STM32编程

Cubemax配置
编程环境：
STM32F103C8T6
RTstudio

配置IIC

1.根据board.h中的指导配置

/*-------------------------- I2C CONFIG BEGIN --------------------------*/

/** if you want to use i2c bus(soft simulate) you can use the following instructions.
 *
 * STEP 1, open i2c driver framework(soft simulate) support in the RT-Thread Settings file
 *
 * STEP 2, define macro related to the i2c bus
 *                 such as     #define BSP_USING_I2C1
 *
 * STEP 3, according to the corresponding pin of i2c port, modify the related i2c port and pin information
 *                 such as     #define BSP_I2C1_SCL_PIN    GET_PIN(port, pin)   ->   GET_PIN(C, 11)
 *                             #define BSP_I2C1_SDA_PIN    GET_PIN(port, pin)   ->   GET_PIN(C, 12)
 */

勾选使用I2C设备驱动，使用GPIO模拟I2C，使用PIN设备驱动程序三项
2.配置完成后编译

描述	资源	路径	位置	类型
'PIN_HIGH' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 57 行	C/C++ 问题
'PIN_HIGH' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 72 行	C/C++ 问题
'PIN_HIGH' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 91 行	C/C++ 问题
'PIN_HIGH' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 182 行	C/C++ 问题
'PIN_LOW' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 76 行	C/C++ 问题
'PIN_LOW' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 95 行	C/C++ 问题
'PIN_LOW' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 178 行	C/C++ 问题
'PIN_MODE_OUTPUT_OD' undeclared (first use in this function)	drv_soft_i2c.c	/ros_9250/drivers	第 54 行	C/C++ 问题
make: *** [drivers/subdir.mk:69: drivers/drv_soft_i2c.o] Error 1	ros_9250		 	C/C++ 问题
'MX_TIM2_Init' defined but not used [-Wunused-function]	main.c	/pwm_1/applications	第 45 行	C/C++ 问题
'return' with a value, in function returning void	rtdef.h	/pwm_1/rt-thread/include	第 270 行	C/C++ 问题
conflicting types for 'Error_Handler'	main.c	/pwm_1/applications	第 102 行	C/C++ 问题
implicit declaration of function 'Error_Handler' [-Wimplicit-function-declaration]	main.c	/pwm_1/applications	第 94 行	C/C++ 问题
implicit declaration of function 'rt_pin_mode' [-Wimplicit-function-declaration]	drv_soft_i2c.c	/ros_9250/drivers	第 54 行	C/C++ 问题
implicit declaration of function 'rt_pin_read' [-Wimplicit-function-declaration]	drv_soft_i2c.c	/ros_9250/drivers	第 107 行	C/C++ 问题
implicit declaration of function 'rt_pin_write' [-Wimplicit-function-declaration]	drv_soft_i2c.c	/ros_9250/drivers	第 57 行	C/C++ 问题
variable 'sClockSourceConfig' set but not used [-Wunused-but-set-variable]	main.c	/pwm_1/applications	第 52 行	C/C++ 问题
variable 'sMasterConfig' set but not used [-Wunused-but-set-variable]	main.c	/pwm_1/applications	第 53 行	C/C++ 问题
each undeclared identifier is reported only once for each function it appears in	drv_soft_i2c.c	/ros_9250/drivers	第 54 行	C/C++ 问题
in expansion of macro 'RT_ERROR'	pwm_test.c	/pwm_1/applications	第 30 行	C/C++ 问题
previous implicit declaration of 'Error_Handler' was here	main.c	/pwm_1/applications	第 94 行	C/C++ 问题

MPU9250的I2C地址
MPU9250的I2C Slave地址为b110100X，其中的X是由AD0引脚的电平来决定的，这样的地址设置，可以在同一I2C总线上连接两个MPU9250的设备

运行Serial-IMU

可执行文件
talker
listen1
serial_example_node1

rosrun serial_msgs serial_example_node1

效果图如下：
IMU频率可达250HZ
在这里插入图片描述

发布IMU话题

从传感器16位原始数据到IMU话题的单位转换

1.陀螺仪转换
首先，确定陀螺仪量程，以-2000~+2000为例
其次，16位能表示的最大数据为FFFF，但最高位F实际为固定数7或-7，是符号位，所以16位数据表示的最大数据为7FFF，即32767，最小数据为-32767.
所以，-32767~32767对应着陀螺仪的-2000 ~ 2000.把32767除以2000，就可以得到16.40，即我们说的灵敏度。
具体计算：例如从陀螺仪读到的数字是1000，那么对应的角速度数据是1000/16.40=61度每秒。
当采用量程为-2000到+2000的范围，把我们从陀螺仪获取的数据做如下处理，就可以用于四元数的姿态解算（用gyro_x来代表从陀螺仪读到的数据）: gyro_x/(16.4057.30)=gyro_x0.001064，单位为弧度每秒。
2.加速度计转换
原理相通，故而，当加速度计量程为-16g ~16g时，灵敏度=32767÷16=2048.
具体计算：例如从加速度计读到的数字是1000，那么对应的加速度数据是1000/2048=0.49g。g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。

编程参考小觅

void publishImu(
      const api::MotionData &data, std::uint32_t seq, ros::Time stamp) {
    if (pub_imu_.getNumSubscribers() == 0)
      return;

    sensor_msgs::Imu msg;

    msg.header.seq = seq;
    msg.header.stamp = stamp;
    msg.header.frame_id = imu_frame_id_;

    // acceleration should be in m/s^2 (not in g's)
    msg.linear_acceleration.x = data.imu->accel[0] * gravity_;
    msg.linear_acceleration.y = data.imu->accel[1] * gravity_;
    msg.linear_acceleration.z = data.imu->accel[2] * gravity_;

    msg.linear_acceleration_covariance[0] = 0;
    msg.linear_acceleration_covariance[1] = 0;
    msg.linear_acceleration_covariance[2] = 0;

    msg.linear_acceleration_covariance[3] = 0;
    msg.linear_acceleration_covariance[4] = 0;
    msg.linear_acceleration_covariance[5] = 0;

    msg.linear_acceleration_covariance[6] = 0;
    msg.linear_acceleration_covariance[7] = 0;
    msg.linear_acceleration_covariance[8] = 0;

    // velocity should be in rad/sec
    msg.angular_velocity.x = data.imu->gyro[0] * M_PI / 180;
    msg.angular_velocity.y = data.imu->gyro[1] * M_PI / 180;
    msg.angular_velocity.z = data.imu->gyro[2] * M_PI / 180;

    msg.angular_velocity_covariance[0] = 0;
    msg.angular_velocity_covariance[1] = 0;
    msg.angular_velocity_covariance[2] = 0;

    msg.angular_velocity_covariance[3] = 0;
    msg.angular_velocity_covariance[4] = 0;
    msg.angular_velocity_covariance[5] = 0;

    msg.angular_velocity_covariance[6] = 0;
    msg.angular_velocity_covariance[7] = 0;
    msg.angular_velocity_covariance[8] = 0;

    pub_imu_.publish(msg);
  }