diff --git a/src/command/handlers/cmd_piezo.c b/src/command/handlers/cmd_piezo.c
index 5c9aa63..4f2f5c1 100644
--- a/src/command/handlers/cmd_piezo.c
+++ b/src/command/handlers/cmd_piezo.c
@@ -139,7 +139,13 @@ int cmd_mtb(const uint8_t *data, uint8_t data_len)
     {
         if (fifo_started)
         {
-            int imu_ret = imu_fifo_read_latest(tx_rim_samples, IMU_FIFO_RIM_TARGET_SAMPLES, &rim_count);
+            imu_ret = imu_fifo_wait_samples(IMU_FIFO_RIM_TARGET_SAMPLES, 500U);
+            if (imu_ret != 0)
+            {
+                DBG_PRINTF("[MTB] fifo wait ret=%d\r\n", imu_ret);
+            }
+
+            imu_ret = imu_fifo_read_latest(tx_rim_samples, IMU_FIFO_RIM_TARGET_SAMPLES, &rim_count);
             if (imu_ret != 0)
             {
                 DBG_PRINTF("[MTB] fifo read fail ret=%d\r\n", imu_ret);
diff --git a/src/drivers/imu/imu_i2c.c b/src/drivers/imu/imu_i2c.c
index ed9df91..fac19b7 100644
--- a/src/drivers/imu/imu_i2c.c
+++ b/src/drivers/imu/imu_i2c.c
@@ -615,6 +615,43 @@ int imu_fifo_start(void)
     return 0;
 }
 
+int imu_fifo_wait_samples(uint16_t target_samples, uint32_t timeout_ms)
+{
+    uint32_t waited_ms = 0U;
+    const uint32_t poll_ms = 10U;
+
+    if (target_samples == 0U)
+    {
+        return 0;
+    }
+
+    if (!fifo_active)
+    {
+        return -EALREADY;
+    }
+
+    while (waited_ms <= timeout_ms)
+    {
+        uint16_t record_count = 0U;
+        int ret = imu_fifo_read_count(&record_count);
+
+        if (ret)
+        {
+            return ret;
+        }
+
+        if (record_count >= target_samples)
+        {
+            return 0;
+        }
+
+        k_msleep(poll_ms);
+        waited_ms += poll_ms;
+    }
+
+    return -ETIMEDOUT;
+}
+
 int imu_fifo_read_latest(uint8_t *sample_bytes,
                          uint16_t max_samples,
                          uint16_t *out_count)
diff --git a/src/drivers/imu/imu_i2c.h b/src/drivers/imu/imu_i2c.h
index fefaa1b..2214552 100644
--- a/src/drivers/imu/imu_i2c.h
+++ b/src/drivers/imu/imu_i2c.h
@@ -65,6 +65,13 @@ int imu_read_temperature_cdeg(int16_t *temp_cdeg);
  */
 int imu_fifo_start(void);
 
+/**
+ * @brief Wait until the running FIFO has at least target_samples records.
+ *
+ * @return 0 if the target is reached, -ETIMEDOUT on timeout, or a negative error.
+ */
+int imu_fifo_wait_samples(uint16_t target_samples, uint32_t timeout_ms);
+
 /**
  * @brief 실행 중인 FIFO에서 최신 샘플 읽기
  *