정렬모드/주기 측정 실패 시 cleanup(Piezo power off, imu fifo stop)

src/command/handlers/cmd_piezo.c

@@ -82,8 +82,6 @@ int cmd_mbb(const uint8_t *data, uint8_t data_len)
         DBG_PRINTF("[MBB] piezo sweep start\r\n");
 
         status = piezo_measure_sweep();
-
-        piezo_power_off();
         
         if (status == ECHO_STATUS_OK)
         {
@@ -100,6 +98,7 @@ int cmd_mbb(const uint8_t *data, uint8_t data_len)
     cmd_send_response_u16("raa:", (uint16_t)status); // 최종 상태 raa: 전송
     DBG_PRINTF("[CMD] mbb status=0x%04X\r\n", status);
 
+    piezo_power_off();
     processing = false;
     return 1;
 }
@@ -144,8 +143,6 @@ int cmd_mtb(const uint8_t *data, uint8_t data_len)
         DBG_PRINTF("[MTB] piezo sweep start\r\n");
         status = piezo_measure_sweep();
 
-        piezo_power_off();
-
         if (status == ECHO_STATUS_OK)
         {
             for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
@@ -173,6 +170,8 @@ int cmd_mtb(const uint8_t *data, uint8_t data_len)
             {
                 DBG_PRINTF("[MTB] fifo read fail ret=%d\r\n", imu_ret);
                 cmd_send_response_rim_error();
+                imu_fifo_stop();
+                piezo_power_off();
                 processing = false;
                 return 1;
             }
@@ -184,6 +183,7 @@ int cmd_mtb(const uint8_t *data, uint8_t data_len)
 
     DBG_PRINTF("[CMD] mtb status=0x%04X rim=%u\r\n", status, rim_count);
 
+    piezo_power_off();
     processing = false;
     return 1;
 }