Штамп времени LSM6DS3, считываемый в FIFO

Question

У меня есть датчик LSM6DS3, подключенный к MCU через SPI. Моя цель - заполнить буфер FIFO следующими наборами данных: Gx, Gy, Gz, XLx, XLy, XLz, Ts, Pd. Я использую водяной знак для генерации прерывания после достижения определенного уровня FIFO.

Кроме того, я использую драйверы ST LSM6DS3 и следовал указаниям приложения AN4650, чтобы включить данные счетчика шагов и отметки времени в FIFO.

Я смог убедиться, что акселерометр, гироскоп, отметки времени и шагомер работают правильно. Вот мой код конфигурации:

void LSM6DS3_FIFO_init(void){
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  /*
    * Check device ID
    */
   lsm6ds3_device_id_get(&dev_ctx, &whoamI);
   if (whoamI != LSM6DS3_ID)
     while(1)
     {
       PRINTF("ERROR \n");
     }

   /*
    * Restore default configuration
    */
   lsm6ds3_reset_set(&dev_ctx, PROPERTY_ENABLE);
   do {
      lsm6ds3_reset_get(&dev_ctx, &rst);
   } while (rst);

   /*
    * Enable Block Data Update
    */
   lsm6ds3_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

   /*
    * Set full scale
    */
   lsm6ds3_xl_full_scale_set(&dev_ctx, LSM6DS3_2g);
   lsm6ds3_gy_full_scale_set(&dev_ctx, LSM6DS3_250dps);

   /*
    * Set Output Data Rate for acc/gyro to 52 Hz
    */
   lsm6ds3_xl_data_rate_set(&dev_ctx, LSM6DS3_XL_ODR_52Hz);
   lsm6ds3_gy_data_rate_set(&dev_ctx, LSM6DS3_XL_ODR_52Hz);

   /*
    * Enable pedometer
    */
  lsm6ds3_pedo_sens_set(&dev_ctx, PROPERTY_ENABLE);

   /*
    * Set Low Resolution Timestamp 
    */
   lsm6ds3_timestamp_res_set(&dev_ctx, LSM6DS3_LSB_6ms4);

   /*
    * Enable timestamp in HW
    */
   lsm6ds3_timestamp_set(&dev_ctx, PROPERTY_ENABLE);

   /*
    * Reset Timestamp
   */
   lsm6ds3_timestamp_rst_set(&dev_ctx);

   /*
    * Set FIFO watermark to a multiple of a pattern
    * in this example we set watermark to 32 pattern
    * which means 32 sequence of:
    * (XL) = 6 bytes (3 word)
    */
   pattern_len = 3;
   pattern_numbers = 28;
   number_of_patterns = 3;
   lsm6ds3_fifo_watermark_set(&dev_ctx, pattern_numbers * pattern_len * number_of_patterns);
   lsm6ds3_fifo_stop_on_wtm_set(&dev_ctx, PROPERTY_ENABLE);

   /*
    * Set FIFO mode to Stream to FIFO
    */
   lsm6ds3_fifo_mode_set(&dev_ctx, LSM6DS3_STREAM_MODE);

   /*
    * Set FIFO sensor decimators
    */
   lsm6ds3_fifo_xl_batch_set(&dev_ctx, LSM6DS3_FIFO_XL_NO_DEC);
   lsm6ds3_fifo_gy_batch_set(&dev_ctx, LSM6DS3_FIFO_XL_NO_DEC);
   lsm6ds3_fifo_dataset_4_batch_set(&dev_ctx, LSM6DS3_FIFO_DS4_NO_DEC);

   /*
    * Enable PEDO/TIMESTAMP as 4th data set
    */

   lsm6ds3_fifo_pedo_batch_set(&dev_ctx, PROPERTY_ENABLE);

   /*
    * Set PEDO FIFO storing mode
    */
   lsm6ds3_fifo_write_trigger_set(&dev_ctx, LSM6DS3_TRG_XL_GY_DRDY);

   /*
    * Set ODR FIFO
    */
   lsm6ds3_fifo_data_rate_set(&dev_ctx, LSM6DS3_FIFO_52Hz);

   /*
    * Enable drdy 75 us pulse: uncomment if interrupt must be pulsed
    */
   lsm6ds3_int_notification_set(&dev_ctx, LSM6DS3_INT_PULSED);

   /*
    * Enable interrupt generation on DRDY INT1 pin
    */
   lsm6ds3_pin_int1_route_get(&dev_ctx, &int_1_reg);
   int_1_reg.int1_fth = PROPERTY_ENABLE;
   int_1_reg.drdy_on_int1 = PROPERTY_DISABLE;
   lsm6ds3_pin_int1_route_set(&dev_ctx, &int_1_reg);
}    

Проблема, с которой я сталкиваюсь, заключается в том, что, когда я пытаюсь очистить FIFO и сохранить данные в буфере, я получаю несогласованные значения меток времени и шагомера:

AXIS_X 90 AXIS_Y -72 AXIS_Z 16747 AXIS_GX 0 AXIS_GY 1535 AXIS_GZ 1535 TIMESTAMP 15376671 PEDOMETER 5977 TROUBLESHOOTING BYTE 12 161, BYTE 13 234, BYTE 14 177, BYTE 15 31 
AXIS_X 116 AXIS_Y -21 AXIS_Z 16841 AXIS_GX 0 AXIS_GY 2303 AXIS_GZ 2303 TIMESTAMP 16561667 PEDOMETER 64670 TROUBLESHOOTING BYTE 12 182, BYTE 13 252, BYTE 14 217, BYTE 15 3 
AXIS_X 100 AXIS_Y -64 AXIS_Z 16814 AXIS_GX 0 AXIS_GY 3071 AXIS_GZ 3071 TIMESTAMP 15356 PEDOMETER 64923 TROUBLESHOOTING BYTE 12 59, BYTE 13 0, BYTE 14 181, BYTE 15 252 
AXIS_X 86 AXIS_Y -55 AXIS_Z 16867 AXIS_GX 0 AXIS_GY 3839 AXIS_GZ 3839 TIMESTAMP 22268 PEDOMETER 64916 TROUBLESHOOTING BYTE 12 86, BYTE 13 0, BYTE 14 81, BYTE 15 252 
AXIS_X 148 AXIS_Y 127 AXIS_Z 16826 AXIS_GX 0 AXIS_GY 4607 AXIS_GZ 4607 TIMESTAMP 19708 PEDOMETER 65040 TROUBLESHOOTING BYTE 12 76, BYTE 13 0, BYTE 14 78, BYTE 15 252 

Я пропускаю настройку при настройке LSM6DS3?