mtb? 커맨드 추가

src/parser.c

@@ -57,6 +57,8 @@ static uint8_t tx_echo_buf[4 + 2 + 2 + (ECHO_ADC_MAX_SAMPLES * 2) + 2];
 static uint8_t tx_bundle_buf[22];
 static uint8_t tx_cfg_buf[16];
 static uint8_t tx_ascii_buf[4 + HW_NO_LENGTH + 2];
+static uint8_t tx_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES];
+static uint8_t tx_rim_buf[4 + 2 + (IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES) + 2];
 static uint8_t g_echo_session;
 static piezo_config_t g_piezo_config = {
     .freq = PIEZO_CFG_FREQ_DEFAULT,
@@ -332,6 +334,33 @@ static void send_response_bundle(uint16_t batt_mv,
     ble_data_send(buf, sizeof(tx_bundle_buf));
 }
 
+static void send_response_rim(const uint8_t *sample_bytes, uint16_t sample_count)
+{
+    uint8_t *buf = tx_rim_buf;
+    uint16_t payload_len;
+
+    if (sample_count > IMU_FIFO_RIM_TARGET_SAMPLES)
+    {
+        sample_count = IMU_FIFO_RIM_TARGET_SAMPLES;
+    }
+
+    buf[0] = 'r'; buf[1] = 'i'; buf[2] = 'm'; buf[3] = ':';
+    buf[4] = (uint8_t)(sample_count >> 8);
+    buf[5] = (uint8_t)(sample_count & 0xFF);
+
+    memcpy(&buf[6], sample_bytes, (size_t)sample_count * IMU_FIFO_SAMPLE_BYTES);
+
+    payload_len = (uint16_t)(6U + (sample_count * IMU_FIFO_SAMPLE_BYTES));
+    uint16_t crc = dr_crc16_compute(buf, payload_len);
+    buf[payload_len] = (uint8_t)(crc & 0xFF);
+    buf[payload_len + 1U] = (uint8_t)(crc >> 8);
+
+    DBG_PRINTF("[MTB] tx rim samples=%u len=%u\r\n",
+               sample_count,
+               (uint16_t)(payload_len + 2U));
+    ble_data_send(buf, (uint16_t)(payload_len + 2U));
+}
+
 static void send_response_piezo_config(const char *tag,
                                        uint16_t freq,
                                        uint16_t cycles,
@@ -1058,6 +1087,76 @@ static int cmd_mbb(const uint8_t *data, uint8_t data_len)
     return 1;
 }
 
+/**
+ * mtb?: piezo sweep + IMU FIFO
+ */
+static int cmd_mtb(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    uint8_t session = g_echo_session++;
+    uint16_t rim_count = 0U;
+    bool fifo_started = false;
+
+    processing = true;
+    power_button_suspend(true);
+    DBG_PRINTF("[MTB] cmd start\r\n");
+
+    int status = ECHO_STATUS_OK;
+    int imu_ret = imu_fifo_start();
+    if (imu_ret != 0)
+    {
+        DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret);
+        status = ECHO_STATUS_IMU;
+    }
+    else
+    {
+        fifo_started = true;
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        status = start_piezo_session();
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        status = perform_piezo_sweep();
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
+        {
+            send_response_echo(session, ch, piezo_channels[ch], piezo_config_get()->samples);
+        }
+    }
+
+    if (fifo_started)
+    {
+        int 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);
+            status = ECHO_STATUS_IMU;
+            rim_count = 0U;
+        }
+    }
+
+    send_response_rim(tx_rim_samples, rim_count);
+
+    piezo_power_off();
+    power_button_suspend(false);
+    send_response_u16("raa:", (uint16_t)status);
+    DBG_PRINTF("[CMD] mtb status=0x%04X rim=%u\r\n", status, rim_count);
+
+    processing = false;
+    return 1;
+}
+
 /**
  * mcf?: piezo 측정 파라미터 읽기
  * 응답 rcf: + 설정값 echo back
@@ -1296,6 +1395,7 @@ static const cmd_entry_t cmd_table[] =
     { "mad?", cmd_mad },
     { "maa?", cmd_maa },
     { "mbb?", cmd_mbb },
+    { "mtb?", cmd_mtb },
     { "mcf?", cmd_mcf },
     { "mcs?", cmd_mcs },
     { "mfv?", cmd_mfv },