一、项目背景
之前在项目中使用3轴加速度传感器利用向量法，判断设备是否发生偏转，应用效果很不错，很多情况下都可以有效判断设备是否发生偏转。但本质上，三轴加速度计只能判断俯仰角和滚动角的变化，偏航角的变化无法检测到，因此尝试使用六轴加速度传感器获取三个姿态角，以寻找新的使用方案。
二、六轴加速度计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网上标准例程。
传感器初始化
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); /*加速度计模拟链带宽选择4000Hz*/ /* 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.读取加速度和角速度
作者每隔80分ms由于初始化中设置的数据更新频率为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, ®); 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); }