lis3mdl连接到lsm6dsox I2C，如何以1kHz的速度读取磁数据？

支持团队。
在我的测试板上，lis3mdl 连接到 lsm6dsox I2C。
从上面的板上，我想读取 1667Hz 的加速度和陀螺仪，1kHz 的磁性。
我参考 lsm6dsox_sensor_hub_fifo_lis2mdl.c 来实现 SW。
但是貌似磁数据很少从FIFO中读取。
SHUB_ODR_ 的最大速度为 104hz。
如何以 1kHz 的速度读取磁数据？
你能检查一下我的配置吗？

• /* Electrical pin configuration */
•     val.sdo_sa0_pull_up = 0;
•     val.aux_sdo_ocs_pull_up = 0;
•     val.int1_int2_push_pull = 0;
•     val.int1_pull_down = 0;
•     lsm6dsox_pin_conf_set(&lsm_ctx, val);
•     /* Check device ID */
•     lsm6dsox_device_id_get(&lsm_ctx, &whoamI);
•     while (whoamI != LSM6DSOX_ID)
•         nrf_delay_ms(10);
•     /* Restore default configuration */
•     lsm6dsox_reset_set(&lsm_ctx, PROPERTY_ENABLE);
•     do {
•         lsm6dsox_reset_get(&lsm_ctx, &rst);
•     } while (rst);
•     /* Disable I3C interface */
•     lsm6dsox_i3c_disable_set(&lsm_ctx, LSM6DSOX_I3C_DISABLE);
•     lis3mdl_device_id_get(&lis_ctx, &whoamI);
•     while (whoamI != LIS3MDL_ID)
•         nrf_delay_ms(10);
•     /* Enable Block Data Update */
•     lis3mdl_block_data_update_set(&lis_ctx, PROPERTY_ENABLE);
•     /* Set device in continuos mode */
•     lis3mdl_operating_mode_set(&lis_ctx, LIS3MDL_CONTINUOUS_MODE);
•     /* Set Output Data Rate */
•     lis3mdl_data_rate_set(&lis_ctx, LIS3MDL_LP_1kHz);// 1/1kHz*(1000msec/1sec) = 1msec
•     /* Set full scale */
•     lis3mdl_full_scale_set(&lis_ctx, LIS3MDL_16_GAUSS);
•     /*
•     * Configure LSM6DSOX FIFO.
•     *
•     *
•     * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
•     * stored in FIFO) to 15 samples. 5 * (Acc + Gyro + Mag)
•     */
•     lsm6dsox_fifo_watermark_set(&lsm_ctx, 15);
•     /* Set FIFO mode to Stream mode (aka Continuous Mode). */
•     lsm6dsox_fifo_mode_set(&lsm_ctx, LSM6DSOX_STREAM_MODE);
•     /*
•     * Enable FIFO batching of Slave0.
•     */
•     lsm6dsox_sh_batch_slave_0_set(&lsm_ctx, PROPERTY_ENABLE);
•     lsm6dsox_sh_data_rate_set(&lsm_ctx, LSM6DSOX_SH_ODR_104Hz);
•     /* Set FIFO batch XL/Gyro ODR to 12.5Hz. */
•     lsm6dsox_fifo_xl_batch_set(&lsm_ctx, LSM6DSOX_GY_BATCHED_AT_1667Hz);
•     lsm6dsox_fifo_gy_batch_set(&lsm_ctx, LSM6DSOX_GY_BATCHED_AT_1667Hz);
•     /*
•     * Prepare sensor hub to read data from external Slave0 continuously
•     * in order to store data in FIFO.
•     */
•     lsm6dsox_sh_cfg_read_t sh_cfg_read;
•     sh_cfg_read.slv_add = LIS3MDLTR_ADDR;
•     sh_cfg_read.slv_subadd = LIS3MDL_OUT_X_L;
•     sh_cfg_read.slv_len = 6;
•     lsm6dsox_sh_slv0_cfg_read(&lsm_ctx, &sh_cfg_read);
•     /* Configure Sensor Hub to read one slave. */
•     lsm6dsox_sh_slave_connected_set(&lsm_ctx, LSM6DSOX_SLV_0);
•     /* Enable I2C Master. */
•     lsm6dsox_sh_master_set(&lsm_ctx, PROPERTY_ENABLE);
•     /* Configure LSM6DSOX. */
•     lsm6dsox_xl_full_scale_set(&lsm_ctx, LSM6DSOX_16g);
•     lsm6dsox_gy_full_scale_set(&lsm_ctx, LSM6DSOX_2000dps);
•     lsm6dsox_block_data_update_set(&lsm_ctx, PROPERTY_ENABLE);
•     lsm6dsox_xl_data_rate_set(&lsm_ctx, LSM6DSOX_XL_ODR_1667Hz);
•     lsm6dsox_gy_data_rate_set(&lsm_ctx, LSM6DSOX_XL_ODR_1667Hz);
•     /* Configure gpiote for the sensors data ready interrupt. */
•     if (!nrf_drv_gpiote_is_init())
•     {
•         err_code = nrf_drv_gpiote_init();
•         APP_ERROR_CHECK(err_code);
•     }