Reading data from the FIFO will result in errors after a period of time.
- January 13, 2025
- 1 reply
- 1676 views
Thank you for solving my problem. Please forgive my poor English.
I am using lsm6dsl sensor. When I use FIFO to read LSM6DSL, the data is correct at the beginning, but later there will be data errors.
configuration :
- XL_ODR_208Hz
- GY_ODR_208Hz
- FIFO_208Hz
- 2g, 2000dps
- XL_ANA_BW_400Hz
- LOW_NOISE_LP_ODR_DIV_100
- BDU Enable
After my machine is turned on, I will run lsm6dsl_freq_reset first to ensure that my LSM6DSL is at 208Hz, but I will not turn on the FIFO. I will execute lsm6dsl_fifo_set through the serial port to turn on the FIFO. You can refer to the file I uploaded
#define PATTERN_LEN ((3 + 3 + 3) * 2)
#define LSM_DATA_PACKAGE_NUM (10)
void lsm6dsl_fifo_set(void)
{
uint8_t temp = 0xAA;
lsm6dsl_int1_route_t int_1_reg;
/* FIFO配置 */
//配置FIFO ODR 208Hz
lsm6dsl_fifo_data_rate_set(&dev_ctx, LSM6DSL_FIFO_208Hz);
//配置降采样率 为0 按照输出ODR直接存储到FIFO
lsm6dsl_fifo_gy_batch_set(&dev_ctx, LSM6DSL_FIFO_GY_NO_DEC);
lsm6dsl_fifo_xl_batch_set(&dev_ctx, LSM6DSL_FIFO_XL_NO_DEC);
//lsm6dsl_fifo_dataset_3_batch_set(&dev_ctx, LSM6DSL_FIFO_DS3_NO_DEC);
//配置FIFO阈值 数据量达到该值时触发中断 测得LSM_DATA_PACKAGE_NUM组就发到SDK
lsm6dsl_fifo_watermark_set(&dev_ctx, PATTERN_LEN * LSM_DATA_PACKAGE_NUM);
//配置FIFO模式, FIFO_MODE_[2:0]位为001，以启用FIFO模式
//lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_FIFO_MODE);
//timestamp count enabled 使能时间戳
lsm6dsl_timestamp_set(&dev_ctx, PROPERTY_ENABLE);
lsm6dsl_timestamp_res_set(&dev_ctx, LSM6DSL_LSB_25us);
//从低时间戳分辨率切换到高分辨率时，必须重置计时器计数
lsm6dsl_write_reg(&dev_ctx, LSM6DSL_TIMESTAMP2_REG, (uint8_t *)&temp, sizeof(temp));
lsm6dsl_fifo_dataset_4_batch_set(&dev_ctx, LSM6DSL_FIFO_DS4_NO_DEC);
//配置阈值中断 enable
lsm6dsl_pin_int1_route_get(&dev_ctx, &int_1_reg);
int_1_reg.int1_fth = PROPERTY_ENABLE;
lsm6dsl_pin_int1_route_set(&dev_ctx, int_1_reg);
//当使用 FIFO 时，CTRL3_C寄存器的 IF_INC 位和 BDU 位必须等于 1。
lsm6dsl_auto_increment_set(&dev_ctx, PROPERTY_ENABLE);
//可以在 FIFO 中存储时间戳和 step counter 数据为第 4 个 FIFO 数据集。
//要使能此功能，必须在 FIFO_CTRL2 寄存器中将 TIMER_PEDO_FIFO_EN bit设置为 1。
lsm6dsl_fifo_pedo_and_timestamp_batch_set(&dev_ctx, PROPERTY_ENABLE);
//且 MASTER_CONFIG 的 DATA_VALID_SEL_FIFO 位设置为 0 时
//数据可以通过两种方式存储在 FIFO 中，具体取决于 FIFO_CTRL2 中 TIMER_PEDO_FIFO_DRDY 位的配置
//当FIFO_CTRL2 的 TIMER_PEDO_FIFO_DRDY 位设置为 0 时，数据以 FIFO_CTRL5 register 中设置的 ODR_FIFO 速率写入 FIFO。
lsm6dsl_fifo_write_trigger_set(&dev_ctx, LSM6DSL_TRG_XL_GY_DRDY);
lsm6dsl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_STREAM_MODE);
}
void lsm6dsl_freq_reset(uint8_t freq)
{
uint8_t temp = 0xAA;
uint16_t FIFOnum = 0;
lsm6dsl_int1_route_t int_1_reg;
lsm6dsl_reset_set(&dev_ctx, PROPERTY_ENABLE);
do {
lsm6dsl_reset_get(&dev_ctx, &rst);
} while (rst);
xil_printf("lsm6d reset operation over\r\n");
IMU_FRQ = freq;
IMU_IRQ = LSM6D_IRQ_OFF;
if(freq == LSM6D_OFF)
{
lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_OFF);
lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_OFF);
//在重新启动 FIFO 模式之前，必须先设置为 Bypass 模式，以便完全清除 FIFO 内容。
lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_BYPASS_MODE);
lsm6dsl_write_reg(&dev_ctx, LSM6DSL_TIMESTAMP2_REG, (uint8_t *)&temp, 1);
lsm6dsl_timestamp_set(&dev_ctx, PROPERTY_DISABLE);
lsm6dsl_data_raw_clean();
return;
}
else if (freq == LSM6D_52Hz) //80ms unit loop:80ms post loop:400ms
{
lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_52Hz);
lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_52Hz);
}
else if (freq == LSM6D_208Hz) //4.81 unit loop:5ms post loop:25ms
{
lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_208Hz);
lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_208Hz);
}
else if (freq == LSM6D_1k66Hz) //0.938ms unit loop:1ms post loop:5ms
{
lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_1k66Hz);
lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_1k66Hz);
}
overflow_count = 0;
data_raw_timestamp_last = 0;
lsm6dsl_xl_full_scale_set(&dev_ctx, LSM6DSL_2g);
lsm6dsl_gy_full_scale_set(&dev_ctx, LSM6DSL_2000dps);
lsm6dsl_xl_filter_analog_set(&dev_ctx, LSM6DSL_XL_ANA_BW_400Hz);
lsm6dsl_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSL_XL_LOW_NOISE_LP_ODR_DIV_100);
}
The LSM6DSL output data is normal at the beginning, but the later data will be wrong, the error time is uncertain, it may be a few seconds later, it may be a few minutes or even tens of minutes later
Here is my reading process
//中断处理除了调试外 禁止打印，会影响中断执行
void lsm6dsl_irq_handle(void *callback_ref)
{
uint8_t wmflag = 0;
int64_t stamp = 0;
uint16_t fifo_num = 0; //FIFO存储的个数
uint16_t num_pattern = 0;
int16_t imu_data_all[9] = {0};
uint64_t data_raw_timestamp_total = 0; //总时间戳
XGpioPs *gpio_irq = (XGpioPs *) callback_ref;
//xil_printf("imu irq!!!!!!\r\n");
if(IMU_IRQ == LSM6D_IRQ_WKU)
{
lsm6dsl_wakeup_handle();
}
else if (IMU_IRQ == LSM6D_IRQ_6D)
{
lsm6dsl_6d_handle();
}
else if (IMU_IRQ == LSM6D_IRQ_FF)
{
lsm6dsl_freefall_handle();
}
else
{
lsm6dsl_fifo_wtm_flag_get(&dev_ctx, &wmflag);
if (wmflag)
{
//温度
lsm6dsl_status_reg_get(&dev_ctx, &dev_reg.status_reg);
if (dev_reg.status_reg.tda)
{
data_raw_temperature = 0;
lsm6dsl_temperature_raw_get(&dev_ctx, &data_raw_temperature);
}
lsm6dsl_fifo_data_level_get(&dev_ctx, &fifo_num);
num_pattern = fifo_num / PATTERN_LEN;
//xil_printf("imu FIFOnum %d!\r\n", FIFOnum);
// 0： Gx 1： Gy 2：Gz 3：XLx 4：XLy 5：XLz 6：Mx 7：My 8：Mz 9: T1 10: T2 11: T3
// 第11位没有数据 因为时间戳是24位的 但是我们需要一起读出来，不然会fifo错乱
for (int i=0; i < num_pattern; i++)
{
memset(imu_data_all, 0x00, sizeof(imu_data_all));
lsm6dsl_fifo_raw_data_get(&dev_ctx, imu_data_all, sizeof(imu_data_all));
//将时间戳从数组中拿出来
//23-16
stamp = (imu_data_all[6] & 0xFF00);
stamp = ((stamp << & 0xFF0000);
//15-8
stamp |= ((imu_data_all[6] << & 0xFF00);
//7-0
stamp |= ((imu_data_all[7] >> & 0xFF);
//xil_printf("stamp : %d!\r\n", (uint32_t)stamp);
//不能只保证data_raw_timestamp_last > stamp, imu时间戳可能会有偶然一次数据不准
if (data_raw_timestamp_last > 0xFF0000 && stamp < 0x15000)
{
overflow_count++;
//xil_printf("overflow_count: %d!\r\n", (uint32_t)overflow_count);
}
data_raw_timestamp_total = (0x1000000 * overflow_count) + stamp;
//xil_printf("%d--%d\r\n", (uint32_t)data_raw_timestamp_total, (uint32_t)(data_raw_timestamp_total >> 32));
data_raw_timestamp_last = stamp;
lsm6dsl_data_raw_buf(imu_data_all, imu_data_all+3, NULL, data_raw_temperature, &data_raw_timestamp_total);
}
//xil_printf("data_raw_timestamp_total : %d!\r\n", (uint32_t)stamp);
}
}
XGpioPs_IntrClearPin(gpio_irq, GPIO_LSM6D_INT);
}
The first three are the ACC data, then the temperature, and finally the timestamp, The temperature data is error-free because it is not read through interrupts and FIFO, but directly from the register:
0.148657 -0.060573 -0.996557 31.8633 "38.79622"
0.148718 -0.060634 -0.996557 31.8633 "38.80108"
0.148718 -0.060634 -0.996618 31.8633 "38.80590"
0.148718 -0.060573 -0.99674 31.8633 "38.81073"
0.148718 -0.060512 -0.996801 31.8633 "38.81555"
0.148779 -0.060451 -0.996801 31.8633 "38.82040"
0.148901 -0.06039 -0.99674 31.8633 "38.82523"
0.148901 -0.060329 -0.996679 31.8633 "38.83005"
0.148962 -0.060329 -0.99674 31.8711 "38.83488"
0.148962 -0.060329 -0.99674 31.8711 "38.83970"
0.148962 -0.060329 -0.99674 31.8711 "38.84455"
0.148962 -0.060268 -0.99674 31.8711 "38.84938"
0.148901 -0.060207 -0.996679 31.8711 "38.85420"
0.14884 -0.060207 -0.996679 31.8711 "38.85903"
0.148779 -0.060207 -0.996618 31.8711 "38.86385"
0.148779 0 -0.000915 31.8711 "419.37918"
-0.046482 0 -0.000976 31.8711 "419.36638"
-1.03029 0 -0.000915 31.8711 "419.37278"
1.9836 0 -0.000976 31.9414 "419.37278"
1.01541 0 -0.000854 31.9414 "419.37278"
0.031598 0 -0.000915 31.9414 "419.37278"
-0.95221 0 -0.000854 31.9414 "419.37918"
-1.93602 0 -0.000915 31.9414 "419.37918"
1.09349 0 -0.000915 31.9414 "419.37278"
0.109678 0 -0.000976 31.9414 "419.37278"
-0.87413 0 -0.000976 31.9414 "419.37918"
-1.85794 0 -0.000976 31.9414 "419.37278"
1.15595 0 -0.000976 31.9414 "419.37918"
I did not show the content of GY, this part is also wrong,If you want to see the detailed data, you can download the file I uploaded to see, the file is wrong after 8047 lines