#智能小车项目(五)MPU9250初始化

IIC地址控制

// ADO0 为0 控制MPU9250的IIC地址为0x68
static void MPU_ADDR_CTRL(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
	/*设置GPIO模式为通用推挽输出*/
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	GPIO_ResetBits(GPIOB, GPIO_Pin_14);
}

IIC引脚初始化


//初始化IIC
void MPU_IIC_Init(void)
{		
	#if ENABLE_I2C_PB10_PB11
	GPIO_InitTypeDef  GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//先使能外设IO PORTC时钟 
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;	 // 端口配置
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 		 //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;		 //IO口速度为50MHz
	GPIO_Init(GPIOB, &GPIO_InitStructure);					 //根据设定参数初始化GPIO 	
	GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11);
	#else			     
	GPIO_InitTypeDef  GPIO_InitStructure;
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//先使能外设IO PORTC时钟 
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13|GPIO_Pin_15;	 // 端口配置
	GPIO_InitStructure.GPIO_Mode =GPIO_Mode_OUT ; 		 //推挽输出
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;		 //IO口速度为50MHz
	GPIO_Init(GPIOB, &GPIO_InitStructure);					 //根据设定参数初始化GPIO 	
	GPIO_SetBits(GPIOB,GPIO_Pin_13|GPIO_Pin_15);
	#endif
}

MPU初始化

static uint8_t MPU_Basic_Init(void)
{
	uint8_t res = 0;
	MPU_ADDR_CTRL();/* 设置IIC地址 */
	MPU_IIC_Init();/* IIC初始化 */
	MPU_Delay_ms(10);

	MPU_Write_Byte(MPU9250_ADDR, MPU_PWR_MGMT1_REG, 0X80); // Reset MPU9250 //复位MPU9250 /* 1-重置所有寄存器和已经配置数据,重置后此位将自动清 0 */
	MPU_Delay_ms(100);										   // Delay 100 ms //延时100ms
	MPU_Write_Byte(MPU9250_ADDR, MPU_PWR_MGMT1_REG, 0X00); // Wake mpu9250 //唤醒MPU9250

	MPU_Set_Gyro_Fsr(1);  // Gyroscope sensor              //陀螺仪传感器,±500dps=±500°/s ±32768 (gyro/32768*500)*PI/180(rad/s)=gyro/3754.9(rad/s)
	MPU_Set_Accel_Fsr(0); // Acceleration sensor           //加速度传感器,±2g=±2*9.8m/s^2 ±32768 accel/32768*19.6=accel/1671.84
	MPU_Set_Rate(50);	  // Set the sampling rate to 50Hz //设置采样率50Hz

	MPU_Write_Byte(MPU9250_ADDR, MPU_INT_EN_REG, 0X00);	   // Turn off all interrupts //关闭所有中断
	MPU_Write_Byte(MPU9250_ADDR, MPU_USER_CTRL_REG, 0X00); // The I2C main mode is off //I2C主模式关闭
	MPU_Write_Byte(MPU9250_ADDR, MPU_FIFO_EN_REG, 0X00);   // Close the FIFO //关闭FIFO
	// The INT pin is low, enabling bypass mode to read the magnetometer directly
	// INT引脚低电平有效,开启bypass模式,可以直接读取磁力计
	MPU_Write_Byte(MPU9250_ADDR, MPU_INTBP_CFG_REG, 0X82);
	// Read the ID of MPU9250 读取MPU9250的ID
	res = MPU_Read_Byte(MPU9250_ADDR, MPU_DEVICE_ID_REG);
	printf("MPU9250 RD ID=0x%02X\n", res);
	if (res == MPU9250_ID) // The device ID is correct //器件ID正确
	{
		MPU_Write_Byte(MPU9250_ADDR, MPU_PWR_MGMT1_REG, 0X01); // Set CLKSEL,PLL X axis as reference //设置CLKSEL,PLL X轴为参考
		MPU_Write_Byte(MPU9250_ADDR, MPU_PWR_MGMT2_REG, 0X00); // Acceleration and gyroscope both work //加速度与陀螺仪都工作
		MPU_Set_Rate(50);									   // Set the sampling rate to 50Hz //设置采样率为50Hz
	}
	else
		return 1;

	res = MPU_Read_Byte(AK8963_ADDR, MAG_WIA); // Read AK8963ID //读取AK8963ID
	printf("AK8963 RD ID=0x%02X\n", res);
	if (res == AK8963_ID)
	{
		MPU_Write_Byte(AK8963_ADDR, MAG_CNTL1, 0X11); // Set AK8963 to single measurement mode //设置AK8963为单次测量模式
	}
	else
		return 1;
	MPU_Delay_ms(30);
	return 0;
}

  • 1
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
MPU-9250 是一款 9 轴惯性测量单元,可以测量加速度、角速度和磁场。在 ROS 小车中,MPU-9250 可以通过 I2C 总线进行数据传输。 以下是一个简单的 C++ 代码示例,用于读取 MPU-9250 的加速度和角速度数据: ```cpp #include <ros/ros.h> #include <sensor_msgs/Imu.h> #include <wiringPiI2C.h> #define MPU9250_ADDRESS 0x68 #define MPU9250_WHO_AM_I 0x75 #define MPU9250_ACCEL_XOUT_H 0x3B #define MPU9250_ACCEL_YOUT_H 0x3D #define MPU9250_ACCEL_ZOUT_H 0x3F #define MPU9250_GYRO_XOUT_H 0x43 #define MPU9250_GYRO_YOUT_H 0x45 #define MPU9250_GYRO_ZOUT_H 0x47 #define MPU9250_PWR_MGMT_1 0x6B #define MPU9250_CONFIG 0x1A #define MPU9250_GYRO_CONFIG 0x1B #define MPU9250_ACCEL_CONFIG 0x1C #define MPU9250_SMPLRT_DIV 0x19 int main(int argc, char** argv) { ros::init(argc, argv, "mpu9250_node"); ros::NodeHandle nh; // 初始化 I2C 总线 int fd = wiringPiI2CSetup(MPU9250_ADDRESS); // 检查 MPU-9250 是否连接成功 int whoami = wiringPiI2CReadReg8(fd, MPU9250_WHO_AM_I); if (whoami != 0x71) { ROS_ERROR("MPU-9250 connection failed: expected 0x71 but got 0x%02X", whoami); return 1; } // 配置 MPU-9250 wiringPiI2CWriteReg8(fd, MPU9250_PWR_MGMT_1, 0x00); wiringPiI2CWriteReg8(fd, MPU9250_CONFIG, 0x03); wiringPiI2CWriteReg8(fd, MPU9250_GYRO_CONFIG, 0x18); wiringPiI2CWriteReg8(fd, MPU9250_ACCEL_CONFIG, 0x18); wiringPiI2CWriteReg8(fd, MPU9250_SMPLRT_DIV, 0x04); // 发布 IMU 数据 ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu/data", 10); while (ros::ok()) { // 读取加速度数据 int16_t ax = wiringPiI2CReadReg16(fd, MPU9250_ACCEL_XOUT_H); int16_t ay = wiringPiI2CReadReg16(fd, MPU9250_ACCEL_YOUT_H); int16_t az = wiringPiI2CReadReg16(fd, MPU9250_ACCEL_ZOUT_H); // 读取角速度数据 int16_t gx = wiringPiI2CReadReg16(fd, MPU9250_GYRO_XOUT_H); int16_t gy = wiringPiI2CReadReg16(fd, MPU9250_GYRO_YOUT_H); int16_t gz = wiringPiI2CReadReg16(fd, MPU9250_GYRO_ZOUT_H); // 转换为 SI 单位 double ax_si = ax / 16384.0; double ay_si = ay / 16384.0; double az_si = az / 16384.0; double gx_si = gx * 0.00013316211; double gy_si = gy * 0.00013316211; double gz_si = gz * 0.00013316211; // 发布 IMU 数据 sensor_msgs::Imu imu_msg; imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = "imu_link"; imu_msg.linear_acceleration.x = ax_si; imu_msg.linear_acceleration.y = ay_si; imu_msg.linear_acceleration.z = az_si; imu_msg.angular_velocity.x = gx_si; imu_msg.angular_velocity.y = gy_si; imu_msg.angular_velocity.z = gz_si; imu_pub.publish(imu_msg); ros::spinOnce(); } return 0; } ``` 该代码首先使用 `wiringPiI2CSetup` 函数初始化 I2C 总线,并检查 MPU-9250 是否连接成功。然后,它使用 `wiringPiI2CWriteReg8` 函数配置 MPU-9250 的寄存器,以启用加速度和角速度传感器,并设置采样率和量程。最后,它使用 `wiringPiI2CReadReg16` 函数读取加速度和角速度数据,并将它们转换为 SI 单位。最后,它将 IMU 数据发布到 ROS 主题中。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值