项目调试记录1：使用6轴加速度计LSM6DS3TR_C获取设备当前姿态角

最新推荐文章于 2024-07-20 15:30:46 发布

大名辰辰

最新推荐文章于 2024-07-20 15:30:46 发布

阅读量5.4k

点赞数 2

文章标签：单片机物联网嵌入式硬件 c语言

本文链接：https://blog.csdn.net/dz221100/article/details/125444706

版权

一、项目背景

之前在项目中使用3轴加速度传感器利用向量法，判断设备是否发生偏转，应用效果很不错，很多情况下都可以有效判断设备是否发生偏转。但是本质上三轴加速度计只能判断俯仰角以及翻滚角的变化，对于偏航角发生变化无法检测到，所以尝试使用6轴加速度传感器，对三种姿态角进行获取，以求寻找新的使用方案。

二、6轴加速度计LSM6DS3TR_C的介绍

LSM6DS3TR_Cs是ST的MEMS传感器系列中的一员，具有一个3D数字加速度计和一个3D数字陀螺仪，加速度满量程范围为 ±2/±4/±8/±16 g，加速度范围±125/±250/±500/±1000/±2000 dps，供电电压1.71V~3.6V，可通过I2C接口或者SPI接口读取传感器数据，官网有完整驱动资料，可供下载，详细资料可参考官网：LSM6DS3TR-C - iNEMO 6DoF inertial measurement unit (IMU), for entry level / mid-tier smart phones and Portable PC platforms - STMicroelectronics

三、调试过程

笔者使用的是单片机的SPI接口调试传感器，SPI初始化这里就不说明，可参考STM32网上标准例程。

1、传感器初始化

	lsm6ds_device_id_get(&dev_ctx,&whoamI);
	printf("Sensor ID:%x...\r\n",whoamI);
	if(whoamI != LSM6DS3TR_C_ID)
		printf("Sensor Error...\r\n");
	else
		printf("Sensor OK...\r\n");
	/* Restore default configuration */
	lsm6ds_reset_set(&dev_ctx, PROPERTY_ENABLE);  /*软件复位*/
	lsm6ds_reset_get(&dev_ctx,&rst);
	printf("rst:%x...\r\n",rst);
	/* Enable Block Data Update */
	lsm6ds_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);/*输出寄存器直到读取MSB和LSB才更新*/
	/* Set Output Data Rate */
	lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_12Hz5); /*加速度输出速率12.5Hz*/
	lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_12Hz5); /*角速度输出速率12.5*/
	/* Set full scale */
	lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_2g);    /*加速度计量程+-2g*/
	lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps); /*角速度量程2000dps*/
	/* Configure filtering chain(No aux interface) */
	/* Accelerometer - analog filter */
	 lsm6ds3tr_c_xl_filter_analog_set(&dev_ctx,
                                   LSM6DS3TR_C_XL_ANA_BW_400Hz); /*加速度计模拟链带宽选择400Hz*/
	 /* Accelerometer - LPF1 + LPF2 path */
	lsm6ds3tr_c_xl_lp2_bandwidth_set(&dev_ctx,    /*加速度复合滤波器默认，低通默认*/
                                   LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100);
	/* Gyroscope - filtering chain */
	lsm6ds3tr_c_gy_band_pass_set(&dev_ctx,    /*角速度默认值*/
                               LSM6DS3TR_C_HP_260mHz_LP1_STRONG);

2、读取加速度以及角速度

笔者这里每隔80ms采集一个点，因为初始化中设置的数据更新频率是125Hz ,一次采集10个点，将这10个点作为互补滤波算法的输入量，计算姿态角。这里也可以做滤波，获取更精确的加速度和角速度。互补滤波算法可在网上找到很多资料，这里也不详述，后面会将工程贴出，供大家参考

lsm6ds3tr_c_status_reg_t reg;
	uint8_t i = 0;
	int16_t  data_raw_acceleration[3] = {0};
	int16_t  data_raw_angular_rate[3] = {0};
	for(i = 0;i < 10;i++)
	{
		lsm6ds3tr_c_status_reg_get(&dev_ctx, &reg);
		if(reg.xlda) /* Read magnetic field data */
		{
			memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
			lsm6ds3tr_c_acceleration_raw_get(&dev_ctx,data_raw_acceleration);
			acceleration_mg[0] = lsm6ds3tr_c_from_fs2g_to_mg(data_raw_acceleration[0]);
			acceleration_mg[1] = lsm6ds3tr_c_from_fs2g_to_mg(data_raw_acceleration[1]);
			acceleration_mg[2] = lsm6ds3tr_c_from_fs2g_to_mg(data_raw_acceleration[2]);
		}
		if(reg.gda)  /* Read magnetic field data */
		{
			memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
			lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx,data_raw_angular_rate);
			angular_rate_mdps[0] = lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate[0]);
			angular_rate_mdps[1] = lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate[1]);
			angular_rate_mdps[2] = lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate[2]);
			
		}
		Motion_Update(angular_rate_mdps[0],angular_rate_mdps[1], angular_rate_mdps[2],acceleration_mg[0], acceleration_mg[1], acceleration_mg[2],
		pitch,roll,yaw);
		HAL_Delay(80);
	}