diff --git a/src/command/handlers/cmd_piezo.c b/src/command/handlers/cmd_piezo.c
index 9b08c2b..44c31e4 100644
--- a/src/command/handlers/cmd_piezo.c
+++ b/src/command/handlers/cmd_piezo.c
@@ -172,7 +172,9 @@ int cmd_mtb(const uint8_t *data, uint8_t data_len)
             if (imu_ret != 0)
             {
                 DBG_PRINTF("[MTB] fifo read fail ret=%d\r\n", imu_ret);
-                rim_count = 0U;
+                cmd_send_response_rim_error();
+                processing = false;
+                return 1;
             }
         }