커맨드 구조 분리

src/command/cmd_common.c

@@ -0,0 +1,311 @@
+/*******************************************************************************
+ * @file    cmd_common.c
+ * @brief   BLE command common helpers
+ ******************************************************************************/
+#include <zephyr/kernel.h>
+#include <zephyr/sys/reboot.h>
+#include <string.h>
+
+#include "cmd_common.h"
+#include "main.h"
+#include "debug_print.h"
+#include "ble_service.h"
+
+/*==============================================================================
+ * Command response state
+ *============================================================================*/
+static uint8_t tx_u16_buf[8];
+static uint8_t tx_imu_buf[18];
+static uint8_t tx_echo_buf[4 + 2 + 2 + (PIEZO_MEASURE_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_id_buf[4 + HW_NO_LENGTH + SERIAL_NO_LENGTH + SERIAL_NO_LENGTH + 2];
+static uint8_t tx_rim_buf[4 + 2 + (IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES) + 2];
+static uint8_t g_echo_session;
+
+uint8_t cmd_next_echo_session(void)
+{
+    return g_echo_session++;
+}
+
+void cmd_reboot_after_response(void)
+{
+    k_msleep(100);
+    sys_reboot(SYS_REBOOT_COLD);
+}
+
+/*==============================================================================
+ * CRC16 (CRC-CCITT, Nordic SDK 호환)
+ *============================================================================*/
+uint16_t cmd_crc16_compute(const uint8_t *p_data, uint32_t size)
+{
+    uint16_t crc = 0xFFFF;
+
+    for (uint32_t i = 0; i < size; i++)
+    {
+        crc = (uint8_t)(crc >> 8) | (crc << 8);
+        crc ^= p_data[i];
+        crc ^= (uint8_t)(crc & 0xFF) >> 4;
+        crc ^= (crc << 8) << 4;
+        crc ^= ((crc & 0xFF) << 4) << 1;
+    }
+    return crc;
+}
+
+bool cmd_get_data_u16_be(const uint8_t *data, uint8_t data_len,
+                            uint8_t word_index, uint16_t *out)
+{
+    uint8_t offset = (uint8_t)(word_index * 2U);
+
+    if ((offset + 1U) >= data_len)
+    {
+        return false;
+    }
+
+    *out = ((uint16_t)data[offset] << 8) | (uint16_t)data[offset + 1U];
+    return true;
+}
+
+void cmd_copy_fixed_ascii(char *dst, size_t dst_len, const char *src, size_t src_len)
+{
+    if (src_len > dst_len)
+    {
+        src_len = dst_len;
+    }
+
+    memset(dst, 0, dst_len);
+    memcpy(dst, src, src_len);
+}
+
+char cmd_ascii_to_lower(char ch)
+{
+    if ((ch >= 'A') && (ch <= 'Z'))
+    {
+        return (char)(ch - 'A' + 'a');
+    }
+
+    return ch;
+}
+
+/*==============================================================================
+ * 응답 패킷 전송
+ *============================================================================*/
+
+/* TAG(4B) + uint16 값(2B) + CRC16(2B) = 8바이트 전송 */
+int cmd_send_response_u16(const char *tag, uint16_t value)
+{
+    uint8_t *buf = tx_u16_buf;
+
+    buf[0] = tag[0];
+    buf[1] = tag[1];
+    buf[2] = tag[2];
+    buf[3] = tag[3];
+    buf[4] = (uint8_t)(value >> 8);
+    buf[5] = (uint8_t)(value & 0xFF);
+
+    uint16_t crc = cmd_crc16_compute(buf, 6);
+    buf[6] = (uint8_t)(crc & 0xFF);
+    buf[7] = (uint8_t)(crc >> 8);
+
+    return ble_data_send(buf, 8);
+}
+
+int cmd_send_response_ascii(const char *tag, const char *value, uint8_t value_len)
+{
+    uint8_t *buf = tx_ascii_buf;
+
+    buf[0] = tag[0];
+    buf[1] = tag[1];
+    buf[2] = tag[2];
+    buf[3] = tag[3];
+    memcpy(&buf[4], value, value_len);
+
+    uint16_t crc = cmd_crc16_compute(buf, (uint32_t)(4U + value_len));
+    buf[4 + value_len] = (uint8_t)(crc & 0xFF);
+    buf[5 + value_len] = (uint8_t)(crc >> 8);
+
+    return ble_data_send(buf, (uint16_t)(6U + value_len));
+}
+
+int cmd_send_response_identity(void)
+{
+    uint8_t *buf = tx_id_buf;
+    char fw_version[SERIAL_NO_LENGTH];
+
+    cmd_copy_fixed_ascii(fw_version, sizeof(fw_version), FIRMWARE_VERSION, strlen(FIRMWARE_VERSION));
+
+    buf[0] = 'r';
+    buf[1] = 'i';
+    buf[2] = 'd';
+    buf[3] = ':';
+    memcpy(&buf[4], HW_NO, HW_NO_LENGTH);
+    memcpy(&buf[4 + HW_NO_LENGTH], SERIAL_NO, SERIAL_NO_LENGTH);
+    memcpy(&buf[4 + HW_NO_LENGTH + SERIAL_NO_LENGTH], fw_version, sizeof(fw_version));
+
+    uint16_t crc = cmd_crc16_compute(buf, (uint32_t)(sizeof(tx_id_buf) - 2U));
+    buf[sizeof(tx_id_buf) - 2U] = (uint8_t)(crc & 0xFF);
+    buf[sizeof(tx_id_buf) - 1U] = (uint8_t)(crc >> 8);
+
+    return ble_data_send(buf, sizeof(tx_id_buf));
+}
+
+int cmd_send_response_tag_echo(const char *tag, const char *echo_tag)
+{
+    uint8_t *buf = tx_ascii_buf;
+
+    buf[0] = tag[0];
+    buf[1] = tag[1];
+    buf[2] = tag[2];
+    buf[3] = tag[3];
+    buf[4] = echo_tag[0];
+    buf[5] = echo_tag[1];
+    buf[6] = echo_tag[2];
+    buf[7] = echo_tag[3];
+
+    uint16_t crc = cmd_crc16_compute(buf, 8);
+    buf[8] = (uint8_t)(crc & 0xFF);
+    buf[9] = (uint8_t)(crc >> 8);
+
+    return ble_data_send(buf, 10);
+}
+
+/*==============================================================================
+ * 응답 패킷 전송 (IMU)
+ *============================================================================*/
+
+/* TAG(4B) + int16×6 빅엔디안(12B) + CRC16(2B) = 18바이트 전송
+ * 기존 format_data() + dr_binary_tx_safe(buf, 8) 방식과 동일한 레이아웃 */
+int cmd_send_response_imu(const int16_t accel[3], const int16_t gyro[3])
+{
+    uint8_t *buf = tx_imu_buf;
+
+    buf[0] = 'r'; buf[1] = 's'; buf[2] = 'p'; buf[3] = ':';
+
+    const int16_t vals[6] ={
+        accel[0], accel[1], accel[2],
+        gyro[0],  gyro[1],  gyro[2]
+    };
+    for (int i = 0; i < 6; i++)
+    {
+        buf[4 + i * 2]     = (uint8_t)((uint16_t)vals[i] >> 8);   /* MSB */
+        buf[4 + i * 2 + 1] = (uint8_t)((uint16_t)vals[i] & 0xFF); /* LSB */
+    }
+
+    uint16_t crc = cmd_crc16_compute(buf, 16);
+    buf[16] = (uint8_t)(crc & 0xFF);
+    buf[17] = (uint8_t)(crc >> 8);
+
+    return ble_data_send(buf, 18);
+}
+
+void cmd_send_response_echo(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples)
+{
+    /* 정적 버퍼 사용 (한 번에 한 명령만 처리) */
+    uint8_t *buf = tx_echo_buf;
+
+    buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
+    buf[4] = session;
+    buf[5] = channel;
+    buf[6] = (uint8_t)(num_samples >> 8);
+    buf[7] = (uint8_t)(num_samples & 0xFF);
+
+    for (uint16_t i = 0; i < num_samples; i++)
+    {
+        buf[8 + i * 2] = (uint8_t)(samples[i] >> 8);
+        buf[9 + i * 2] = (uint8_t)(samples[i] & 0xFF);
+    }
+
+    uint16_t payload_len = 4 + 2 + 2 + (num_samples * 2);
+    uint16_t crc = cmd_crc16_compute(buf, payload_len);
+    buf[payload_len] = (uint8_t)(crc & 0xFF);
+    buf[payload_len + 1] = (uint8_t)(crc >> 8);
+
+    ble_data_send(buf, payload_len + 2);
+}
+
+void cmd_send_response_bundle(uint16_t batt_mv, const int16_t accel[3], const int16_t gyro[3], int16_t temp_cdeg)
+{
+    uint8_t *buf = tx_bundle_buf;
+
+    buf[0] = 'r'; buf[1] = 'b'; buf[2] = 'b'; buf[3] = ':';
+    buf[4] = (uint8_t)(batt_mv >> 8);
+    buf[5] = (uint8_t)(batt_mv & 0xFF);
+
+    const int16_t imu_vals[6] = {
+        accel[0], accel[1], accel[2],
+        gyro[0],  gyro[1],  gyro[2]
+    };
+
+    for (int i = 0; i < 6; i++)
+    {
+        buf[6 + i * 2] = (uint8_t)((uint16_t)imu_vals[i] >> 8);
+        buf[7 + i * 2] = (uint8_t)((uint16_t)imu_vals[i] & 0xFF);
+    }
+
+    buf[18] = (uint8_t)((uint16_t)temp_cdeg >> 8);
+    buf[19] = (uint8_t)((uint16_t)temp_cdeg & 0xFF);
+
+    uint16_t crc = cmd_crc16_compute(buf, 20);
+    buf[20] = (uint8_t)(crc & 0xFF);
+    buf[21] = (uint8_t)(crc >> 8);
+
+    ble_data_send(buf, sizeof(tx_bundle_buf));
+}
+
+void cmd_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 = cmd_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));
+}
+
+void cmd_send_response_piezo_config(const char *tag, uint16_t freq, uint16_t cycles, uint16_t avg, uint16_t delay_us, uint16_t samples)
+{
+    uint8_t *buf = tx_cfg_buf;
+
+    buf[0] = tag[0];
+    buf[1] = tag[1];
+    buf[2] = tag[2];
+    buf[3] = tag[3];
+    buf[4] = (uint8_t)(freq >> 8);
+    buf[5] = (uint8_t)(freq & 0xFF);
+    buf[6] = (uint8_t)(cycles >> 8);
+    buf[7] = (uint8_t)(cycles & 0xFF);
+    buf[8] = (uint8_t)(avg >> 8);
+    buf[9] = (uint8_t)(avg & 0xFF);
+    buf[10] = (uint8_t)(delay_us >> 8);
+    buf[11] = (uint8_t)(delay_us & 0xFF);
+    buf[12] = (uint8_t)(samples >> 8);
+    buf[13] = (uint8_t)(samples & 0xFF);
+
+    uint16_t crc = cmd_crc16_compute(buf, 14);
+    buf[14] = (uint8_t)(crc & 0xFF);
+    buf[15] = (uint8_t)(crc >> 8);
+
+    ble_data_send(buf, sizeof(tx_cfg_buf));
+}
+
+/*==============================================================================
+ * 커맨드 핸들러
+ *============================================================================*/
+
+/* msn? → 배터리 전압 측정 → rsn: + mV */

src/command/cmd_common.h

@@ -0,0 +1,32 @@
+/*******************************************************************************
+ * @file    cmd_common.h
+ * @brief   BLE command common helpers
+ ******************************************************************************/
+#ifndef CMD_COMMON_H__
+#define CMD_COMMON_H__
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include "imu_i2c.h"
+#include "piezo_measure.h"
+
+void cmd_reboot_after_response(void);
+uint16_t cmd_crc16_compute(const uint8_t *p_data, uint32_t size);
+bool cmd_get_data_u16_be(const uint8_t *data, uint8_t data_len, uint8_t word_index, uint16_t *out);
+void cmd_copy_fixed_ascii(char *dst, size_t dst_len, const char *src, size_t src_len);
+char cmd_ascii_to_lower(char ch);
+uint8_t cmd_next_echo_session(void);
+
+int cmd_send_response_u16(const char *tag, uint16_t value);
+int cmd_send_response_ascii(const char *tag, const char *value, uint8_t value_len);
+int cmd_send_response_identity(void);
+int cmd_send_response_tag_echo(const char *tag, const char *echo_tag);
+int cmd_send_response_imu(const int16_t accel[3], const int16_t gyro[3]);
+void cmd_send_response_echo(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples);
+void cmd_send_response_bundle(uint16_t batt_mv, const int16_t accel[3], const int16_t gyro[3], int16_t temp_cdeg);
+void cmd_send_response_rim(const uint8_t *sample_bytes, uint16_t sample_count);
+void cmd_send_response_piezo_config(const char *tag, uint16_t freq, uint16_t cycles, uint16_t avg, uint16_t delay_us, uint16_t samples);
+
+#endif /* CMD_COMMON_H__ */

src/command/cmd_table.c

@@ -0,0 +1,41 @@
+/*******************************************************************************
+ * @file    cmd_table.c
+ * @brief   BLE command table
+ ******************************************************************************/
+#include "cmd_table.h"
+#include "handlers/cmd_device.h"
+#include "handlers/cmd_info.h"
+#include "handlers/cmd_piezo.h"
+#include "handlers/cmd_sensor.h"
+
+const cmd_entry_t cmd_table[] =
+{
+    { "msq?", cmd_msq },
+    { "mss?", cmd_mss },
+    { "msr?", cmd_msr },
+    { "mdf?", cmd_mdf },
+    { "msn?", cmd_msn },
+    { "mls?", cmd_mls },
+    { "msp?", cmd_msp },
+    { "mst?", cmd_mst },
+    { "mpa?", cmd_mpa },
+    { "mpb?", cmd_mpb },
+    { "mpc?", cmd_mpc },
+    { "mec?", cmd_mec },
+    { "mad?", cmd_mad },
+    { "maa?", cmd_maa },
+    { "mbb?", cmd_mbb },
+    { "mtb?", cmd_mtb },
+    { "mcf?", cmd_mcf },
+    { "mcs?", cmd_mcs },
+    { "mid?", cmd_mid },
+    { "mfv?", cmd_mfv },
+    { "mwh?", cmd_mwh },
+    { "mrh?", cmd_mrh },
+    { "mws?", cmd_mws },
+    { "mrs?", cmd_mrs },
+    { "mpz?", cmd_mpz },
+    { "mqz?", cmd_mqz },
+};
+
+const int cmd_table_size = (int)(sizeof(cmd_table) / sizeof(cmd_table[0]));

src/command/cmd_table.h

@@ -0,0 +1,19 @@
+/*******************************************************************************
+ * @file    cmd_table.h
+ * @brief   BLE command table
+ ******************************************************************************/
+#ifndef CMD_TABLE_H__
+#define CMD_TABLE_H__
+
+#include <stdint.h>
+
+typedef struct
+{
+    char tag[5];
+    int (*handler)(const uint8_t *data, uint8_t data_len);
+} cmd_entry_t;
+
+extern const cmd_entry_t cmd_table[];
+extern const int cmd_table_size;
+
+#endif /* CMD_TABLE_H__ */

src/command/handlers/cmd_device.c

@@ -0,0 +1,131 @@
+/*******************************************************************************
+ * @file    cmd_device.c
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#include <zephyr/kernel.h>
+#include <zephyr/sys/util.h>
+#if IS_ENABLED(CONFIG_BT_SMP)
+#include <zephyr/bluetooth/bluetooth.h>
+#include <zephyr/bluetooth/conn.h>
+#endif
+
+#include "cmd_common.h"
+#include "main.h"
+#include "debug_print.h"
+#include "ble_service.h"
+#include "cmd_device.h"
+
+int cmd_msq(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    int err = cmd_send_response_u16("rsq:", 0x0000);
+    if (err)
+    {
+        DBG_ERR("[CMD] msq tx failed err=%d\r\n", err);
+    }
+
+    DBG_PRINTF("[CMD] msq -> power off\r\n");
+    sleep_mode_enter_reason("cmd-msq");
+    return 1;
+}
+
+int cmd_mss(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    int err = cmd_send_response_u16("rss:", 0x0000);
+    if (err)
+    {
+        DBG_ERR("[CMD] mss tx failed err=%d\r\n", err);
+    }
+
+    DBG_PRINTF("[CMD] mss -> reboot\r\n");
+    cmd_reboot_after_response();
+    return 1;
+}
+
+int cmd_msr(const uint8_t *data, uint8_t data_len)
+{
+    uint16_t status = 0x0000;
+
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    int tx_err = cmd_send_response_u16("rsr:", status);
+    if (tx_err)
+    {
+        DBG_ERR("[CMD] msr tx failed err=%d\r\n", tx_err);
+        return 1;
+    }
+
+    /*
+     * 현재 연결의 bond/security 정보를 먼저 지우면 같은 연결에서 rsr: 응답을
+     * 보낼 수 없음. 응답 전송 완료 후 bond 삭제와 reboot 진행 필요
+     */
+    k_msleep(100);
+
+#if IS_ENABLED(CONFIG_BT_SMP)
+    int err = bt_unpair(BT_ID_DEFAULT, NULL);
+    if (err)
+    {
+        status = (uint16_t)(-err);
+        DBG_ERR("[CMD] msr bt_unpair failed err=%d\r\n", err);
+    }
+    else
+    {
+        bond_data_delete = true;
+        DBG_PRINTF("[CMD] msr bond data deleted\r\n");
+    }
+#else
+    bond_data_delete = true;
+    DBG_PRINTF("[CMD] msr bond delete skipped (BT_SMP disabled)\r\n");
+#endif
+
+    if (status == 0x0000)
+    {
+        DBG_PRINTF("[CMD] msr complete -> reboot\r\n");
+    }
+
+    cmd_reboot_after_response();
+    return 1;
+}
+
+/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */
+int cmd_mdf(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    uint16_t status = ble_dfu_advertising_is_enabled() ? 0xFFFF : 0x0000;
+
+    /* DFU advertising 전환 중 연결이 끊겨도 전원 래치가 유지되도록 먼저 ON 상태를 확정한다. */
+    device_power_keep_on();
+
+    int tx_err = cmd_send_response_u16("rdf:", status);
+    if (tx_err)
+    {
+        DBG_ERR("[CMD] mdf tx failed err=%d\r\n", tx_err);
+        return 1;
+    }
+
+    if (status == 0x0000)
+    {
+        /* rdf: 응답이 나간 뒤 연결 해제와 SMP 광고 전환 실행 */
+        k_msleep(100);
+        int err = ble_dfu_advertising_enable();
+        if (err)
+        {
+            DBG_ERR("[CMD] mdf enable failed err=%d\r\n", err);
+        }
+    }
+    else
+    {
+        DBG_PRINTF("[CMD] mdf rejected: already in DFU advertising\r\n");
+    }
+
+    return 1;
+}
+

src/command/handlers/cmd_device.h

@@ -0,0 +1,15 @@
+/*******************************************************************************
+ * @file    cmd_device.h
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#ifndef CMD_DEVICE_H__
+#define CMD_DEVICE_H__
+
+#include <stdint.h>
+
+int cmd_msq(const uint8_t *data, uint8_t data_len);
+int cmd_mss(const uint8_t *data, uint8_t data_len);
+int cmd_msr(const uint8_t *data, uint8_t data_len);
+int cmd_mdf(const uint8_t *data, uint8_t data_len);
+
+#endif /* CMD_DEVICE_H__ */

src/command/handlers/cmd_info.c

@@ -0,0 +1,177 @@
+/*******************************************************************************
+ * @file    cmd_info.c
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#include <zephyr/sys/util.h>
+#include <string.h>
+
+#include "cmd_common.h"
+#include "main.h"
+#include "debug_print.h"
+#include "app_nvs.h"
+#include "cmd_info.h"
+
+int cmd_mfv(const uint8_t *data, uint8_t data_len)
+{
+    char fw_version[SERIAL_NO_LENGTH];
+    int err;
+
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    cmd_copy_fixed_ascii(fw_version, sizeof(fw_version), FIRMWARE_VERSION, strlen(FIRMWARE_VERSION));
+    err = cmd_send_response_ascii("rfv:", fw_version, sizeof(fw_version));
+    if (err)
+    {
+        DBG_ERR("[CMD] mfv tx failed err=%d\r\n", err);
+    }
+    return 1;
+}
+
+int cmd_mid(const uint8_t *data, uint8_t data_len)
+{
+    int err;
+
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    err = cmd_send_response_identity();
+    if (err)
+    {
+        DBG_ERR("[CMD] mid tx failed err=%d\r\n", err);
+    }
+    return 1;
+}
+
+int cmd_mwh(const uint8_t *data, uint8_t data_len)
+{
+    if (data_len < HW_NO_LENGTH)
+    {
+        cmd_send_response_u16("rwh:", 0xFFFF);
+        return 1;
+    }
+
+    memset(HW_NO, 0, sizeof(HW_NO));
+    memcpy(HW_NO, data, HW_NO_LENGTH);
+    int err = app_nvs_save_hw_no(HW_NO);
+    if (err)
+    {
+        cmd_send_response_u16("rwh:", 0xFFFD);
+        return 1;
+    }
+
+    cmd_send_response_ascii("rwh:", HW_NO, HW_NO_LENGTH);
+    return 1;
+}
+
+int cmd_mrh(const uint8_t *data, uint8_t data_len)
+{
+    int err;
+
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    err = cmd_send_response_ascii("rrh:", HW_NO, HW_NO_LENGTH);
+    if (err)
+    {
+        DBG_ERR("[CMD] mrh tx failed err=%d\r\n", err);
+    }
+    return 1;
+}
+
+int cmd_mws(const uint8_t *data, uint8_t data_len)
+{
+    if (data_len < SERIAL_NO_LENGTH)
+    {
+        cmd_send_response_u16("rws:", 0xFFFF);
+        return 1;
+    }
+
+    memset(SERIAL_NO, 0, sizeof(SERIAL_NO));
+    memcpy(SERIAL_NO, data, SERIAL_NO_LENGTH);
+    int err = app_nvs_save_serial_no(SERIAL_NO);
+    if (err)
+    {
+        cmd_send_response_u16("rws:", 0xFFFD);
+        return 1;
+    }
+
+    cmd_send_response_ascii("rws:", SERIAL_NO, SERIAL_NO_LENGTH);
+    return 1;
+}
+
+int cmd_mrs(const uint8_t *data, uint8_t data_len)
+{
+    int err;
+
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    err = cmd_send_response_ascii("rrs:", SERIAL_NO, SERIAL_NO_LENGTH);
+    if (err)
+    {
+        DBG_ERR("[CMD] mrs tx failed err=%d\r\n", err);
+    }
+    return 1;
+}
+
+int cmd_mpz(const uint8_t *data, uint8_t data_len)
+{
+    if (data_len < PASSKEY_LENGTH)
+    {
+        cmd_send_response_u16("rpz:", 0xFFFF);
+        return 1;
+    }
+
+    if (m_passkey_changed != 0U)
+    {
+        /* 패스키가 변경된 경우 다시 바꿀 수 없음(FFFF 반환) */
+        cmd_send_response_u16("rpz:", 0xFFFF);
+        DBG_PRINTF("[CMD] mpz: passkey already changed\r\n");
+        return 1;
+    }
+
+    /* NVS 저장이 모두 성공한 뒤 RAM 값 확정 */
+    char new_passkey[PASSKEY_BUF_SIZE];
+    memset(new_passkey, 0, sizeof(new_passkey));
+    memcpy(new_passkey, data, PASSKEY_LENGTH);
+
+    int err = app_nvs_save_passkey(new_passkey);
+    if (err)
+    {
+        cmd_send_response_u16("rpz:", 0xFFFD);
+        return 1;
+    }
+
+    /* 변경 완료 플래그 세트 : 다음 mpz? 명령 차단 */
+    err = app_nvs_save_passkey_changed(1U);
+    if (err)
+    {
+        cmd_send_response_u16("rpz:", 0xFFFD);
+        return 1;
+    }
+
+    memset(m_static_passkey, 0, sizeof(m_static_passkey));
+    memcpy(m_static_passkey, new_passkey, PASSKEY_LENGTH);
+    m_passkey_changed = 1U;
+
+    cmd_send_response_ascii("rpz:", m_static_passkey, PASSKEY_LENGTH);
+    return 1;
+}
+
+int cmd_mqz(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    cmd_send_response_ascii("rqz:", m_static_passkey, PASSKEY_LENGTH);
+    return 1;
+}
+
+/*
+ * LED 상태 변경 명령
+ * mls?: 앱에서 전달한 LED 상태값을 적용하고 rls로 echo back
+ * - 파라미터 부족: 0xFFFF
+ * - 범위 초과: 0xFFFE
+ * - LED_STATE_OFF 진입 시 IMU FIFO 정지
+ */

src/command/handlers/cmd_info.h

@@ -0,0 +1,19 @@
+/*******************************************************************************
+ * @file    cmd_info.h
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#ifndef CMD_INFO_H__
+#define CMD_INFO_H__
+
+#include <stdint.h>
+
+int cmd_mfv(const uint8_t *data, uint8_t data_len);
+int cmd_mid(const uint8_t *data, uint8_t data_len);
+int cmd_mwh(const uint8_t *data, uint8_t data_len);
+int cmd_mrh(const uint8_t *data, uint8_t data_len);
+int cmd_mws(const uint8_t *data, uint8_t data_len);
+int cmd_mrs(const uint8_t *data, uint8_t data_len);
+int cmd_mpz(const uint8_t *data, uint8_t data_len);
+int cmd_mqz(const uint8_t *data, uint8_t data_len);
+
+#endif /* CMD_INFO_H__ */

src/command/handlers/cmd_piezo.c

@@ -0,0 +1,500 @@
+/*******************************************************************************
+ * @file    cmd_piezo.c
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#include <zephyr/sys/util.h>
+#include <limits.h>
+
+#include "cmd_common.h"
+#include "main.h"
+#include "debug_print.h"
+#include "battery_adc.h"
+#include "imu_i2c.h"
+#include "piezo.h"
+#include "piezo_measure.h"
+#include "cmd_piezo.h"
+
+static uint8_t tx_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES];
+
+int cmd_mpa(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    DBG_CORE("[MPA] enter\r\n");
+    if (piezo_init() != 0)
+    {
+        DBG_ERR("[MPA] piezo_init failed\r\n");
+        cmd_send_response_u16("rpa:", 0);
+        return 1;
+    }
+
+    DBG_CORE("[MPA] piezo_init ok\r\n");
+    piezo_power_on();
+    DBG_CORE("[MPA] power on\r\n");
+    cmd_send_response_u16("rpa:", 1);
+    DBG_CORE("[MPA] response sent\r\n");
+    return 1;
+}
+
+int cmd_mpb(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    /* mpb?: piezo TX/RX 전원 레일을 끈다. */
+    DBG_CORE("[MPB] enter\r\n");
+    if (piezo_init() != 0)
+    {
+        DBG_ERR("[MPB] piezo_init failed\r\n");
+        cmd_send_response_u16("rpb:", 0);
+        return 1;
+    }
+
+    DBG_CORE("[MPB] piezo_init ok\r\n");
+    piezo_power_off();
+    DBG_CORE("[MPB] power off\r\n");
+    cmd_send_response_u16("rpb:", 1);
+    DBG_CORE("[MPB] response sent\r\n");
+    return 1;
+}
+
+/*
+ * 테스트용
+ * mpc?: burst만 한 번 발생시키는 테스트 명령
+ * echo를 읽지 않고 초음파가 나가는지만 볼 때 사용
+ */
+int cmd_mpc(const uint8_t *data, uint8_t data_len)
+{
+    const piezo_config_t *cfg = piezo_config_get();
+    uint16_t cycles = cfg->cycles;
+    uint16_t freq_option = cfg->freq;
+    uint16_t piezo_ch = 0;
+
+    cmd_get_data_u16_be(data, data_len, 0, &cycles);
+    cmd_get_data_u16_be(data, data_len, 1, &freq_option);
+    cmd_get_data_u16_be(data, data_len, 2, &piezo_ch);
+
+    ARG_UNUSED(freq_option);
+
+    if ((cycles < 3U) || (cycles > 7U))
+    {
+        cmd_send_response_u16("rpc:", 2);
+        return 1;
+    }
+
+    power_button_suspend(true);
+
+    if (piezo_init() != 0)
+    {
+        power_button_suspend(false);
+        cmd_send_response_u16("rpc:", 0);
+        return 1;
+    }
+
+    piezo_power_on();
+
+    if (piezo_select_channel((uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS)) != 0)
+    {
+        piezo_power_off();
+        power_button_suspend(false);
+        cmd_send_response_u16("rpc:", 0);
+        return 1;
+    }
+
+    /*
+     * 현재 Zephyr 포팅본은 2.1MHz SW burst 하나만 구현되어 있음
+     * 레거시의 freq_option 값은 받아두되, 아직은 같은 burst 함수로 처리
+     */
+    piezo_burst_sw((uint8_t)cycles);
+    piezo_power_off();
+    power_button_suspend(false);
+
+    cmd_send_response_u16("rpc:", cycles);
+    return 1;
+}
+
+/*
+ * 테스트용
+ * mec?: 단일 채널 burst + echo capture
+ */
+int cmd_mec(const uint8_t *data, uint8_t data_len)
+{
+    const piezo_config_t *cfg = piezo_config_get();
+    uint16_t freq_option = cfg->freq;
+    uint16_t delay_us = cfg->delay_us;
+    uint16_t num_samples = cfg->samples;
+    uint16_t cycles = cfg->cycles;
+    uint16_t averaging = cfg->avg;
+    uint16_t piezo_ch = 0;
+    uint8_t session = cmd_next_echo_session();
+
+    cmd_get_data_u16_be(data, data_len, 0, &freq_option);
+    cmd_get_data_u16_be(data, data_len, 1, &delay_us);
+    cmd_get_data_u16_be(data, data_len, 2, &num_samples);
+    cmd_get_data_u16_be(data, data_len, 3, &cycles);
+    cmd_get_data_u16_be(data, data_len, 4, &averaging);
+    cmd_get_data_u16_be(data, data_len, 5, &piezo_ch);
+
+    ARG_UNUSED(freq_option);
+
+    if (num_samples > PIEZO_MEASURE_MAX_SAMPLES)
+    {
+        num_samples = PIEZO_MEASURE_MAX_SAMPLES;
+    }
+    if ((cycles < 3U) || (cycles > 7U))
+    {
+        cycles = PIEZO_SW_BURST_CYCLES;
+    }
+    if (averaging == 0U)
+    {
+        averaging = 1U;
+    }
+
+    processing = true;
+    power_button_suspend(true);
+
+    int status = piezo_measure_start_session();
+    if (status == ECHO_STATUS_OK)
+    {
+        status = piezo_measure_single_capture((uint8_t)cycles, delay_us, num_samples, averaging, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        cmd_send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), piezo_measure_single_buffer(), num_samples);
+    }
+
+    piezo_power_off();
+    power_button_suspend(false);
+    cmd_send_response_u16("raa:", (uint16_t)status);
+    processing = false;
+    return 1;
+}
+
+/*
+ * 테스트용
+ * mad?: 단일 채널 echo capture만 수행
+ * - piezo burst는 하지 않고 echo ADC만 수행
+ * - echo 샘플을 모은 뒤 reb: 패킷 1개 전송
+ * - raa: 상태값은 전체 작업 성공/실패 요약
+ */
+int cmd_mad(const uint8_t *data, uint8_t data_len)
+{
+    const piezo_config_t *cfg = piezo_config_get();
+    uint16_t num_samples = cfg->samples;
+    uint16_t averaging = cfg->avg;
+    uint16_t piezo_ch = 0;
+    uint8_t session = cmd_next_echo_session();
+
+    cmd_get_data_u16_be(data, data_len, 2, &num_samples);
+    cmd_get_data_u16_be(data, data_len, 4, &averaging);
+    cmd_get_data_u16_be(data, data_len, 5, &piezo_ch);
+
+    if (num_samples > PIEZO_MEASURE_MAX_SAMPLES)
+    {
+        num_samples = PIEZO_MEASURE_MAX_SAMPLES;
+    }
+    if (averaging == 0U)
+    {
+        averaging = 1U;
+    }
+
+    processing = true;
+    power_button_suspend(true);
+
+    int status = piezo_measure_start_session();
+    if (status == ECHO_STATUS_OK)
+    {
+        status = piezo_measure_adc_only_capture(num_samples, averaging, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        cmd_send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), piezo_measure_single_buffer(), num_samples);
+    }
+
+    piezo_power_off();
+    power_button_suspend(false);
+    cmd_send_response_u16("raa:", (uint16_t)status);
+    DBG_PRINTF("[CMD] mad status=0x%04X ch=%u samples=%u avg=%u\r\n",
+               status,
+               (uint16_t)(piezo_ch % PIEZO_NUM_CHANNELS),
+               num_samples,
+               averaging);
+
+    processing = false;
+    return 1;
+}
+
+/*
+ * 테스트용(이전 정렬모드)
+ * maa?:
+ * - piezo 6채널 burst 순서대로 쏘고
+ * - echo 샘플을 채널별로 모은 뒤
+ * - reb: 패킷 6개 전송
+ * - raa: 상태값은 전체 작업 성공/실패 요약
+ */
+int cmd_maa(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    uint8_t session = cmd_next_echo_session();
+
+    processing = true;
+    power_button_suspend(true);
+
+    int status = piezo_measure_start_session();
+    if (status == ECHO_STATUS_OK)
+    {
+        status = piezo_measure_sweep();
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
+        {
+            cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
+        }
+    }
+
+    piezo_power_off();
+    power_button_suspend(false);
+    cmd_send_response_u16("raa:", (uint16_t)status);
+    DBG_PRINTF("[CMD] maa status=0x%04X\r\n", status);
+
+    processing = false;
+    return 1;
+}
+
+/*
+ * 전체 측정 명령
+ * mbb?: piezo echo sweep + 배터리 + IMU + IMU 온도 측정
+ * - 성공 시 rbb 1개, reb 6개, raa 상태값 전송
+ * - 실패 시 rbb/reb 생략, raa에 실패 원인 코드 전송
+ * - processing=true 구간에서는 내부 모니터의 I2C 접근을 막는 기준으로 사용
+ */
+int cmd_mbb(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    uint8_t session = cmd_next_echo_session();
+
+    int16_t accel[3];
+    int16_t gyro[3];
+
+    /*
+     * mbb?는 이 프로젝트에서 가장 무거운 명령 중 하나다.
+     *
+     * 순서:
+     * 1. piezo/echo ADC 준비
+     * 2. 6채널 echo sweep
+     * 3. battery / imu / temp 추가 측정
+     * 4. rbb: 1개 전송
+     * 5. reb: 6개 전송
+     * 6. raa: 최종 상태 전송
+     *
+     * 중간에 하나라도 실패하면 status에 에러 코드를 넣고,
+     * 성공한 경우에만 묶음 응답(rbb + reb)을 보낸다.
+     */
+    processing = true;
+    power_button_suspend(true);
+    DBG_PRINTF("[MBB] cmd start\r\n");
+
+    int status = piezo_measure_start_session();
+    if (status == ECHO_STATUS_OK)
+    {
+        status = piezo_measure_sweep();
+    }
+
+    int batt_mv = -1;
+    int16_t temp_cdeg = INT16_MIN;
+
+    if (status == ECHO_STATUS_OK)
+    {
+        /* echo sweep 성공 후에만 부가 센서값 읽기
+         * - rbb에 포함되는 배터리 전압
+         * - 실패 시 ECHO_STATUS_BATT로 종료
+         */
+        DBG_PRINTF("[MBB] battery read\r\n");
+        batt_mv = battery_read_mv();
+        if (batt_mv < 0)
+        {
+            status = ECHO_STATUS_BATT;
+        }
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        /* rbb에 포함되는 IMU 6축 값과 IMU 내부 온도 읽기
+         * - direct read 경로 사용
+         * - 실패 시 ECHO_STATUS_IMU로 종료
+         */
+        DBG_PRINTF("[MBB] imu/temp read\r\n");
+        if (imu_read_with_temperature(accel, gyro, &temp_cdeg) != 0)
+        {
+            status = ECHO_STATUS_IMU;
+        }
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        /* mbb? 성공 응답값으로 연속 측정 보호 판정 갱신
+         * - 실패한 측정은 호출하지 않아 기존 저전압/고온 카운트 유지
+         * - rbb에 실리는 배터리/IMU 온도 값과 같은 값을 사용
+         */
+        battery_protection_record_continuous(batt_mv, temp_cdeg);
+
+        /* 측정 성공 응답 순서
+         * - rbb: 배터리 + IMU + IMU 온도 요약
+         * - reb: 채널별 echo raw 데이터
+         * - raa: 마지막 상태 코드
+         */
+        DBG_PRINTF("[MBB] response tx start\r\n");
+        cmd_send_response_bundle((uint16_t)batt_mv, accel, gyro, temp_cdeg);
+
+        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
+        {
+            /* reb: 채널별 raw echo 파형 */
+            DBG_PRINTF("[MBB] tx reb ch=%d\r\n", ch);
+            cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
+        }
+    }
+
+    piezo_power_off();
+    power_button_suspend(false);
+    DBG_PRINTF("[MBB] power off\r\n");
+    cmd_send_response_u16("raa:", (uint16_t)status);
+    DBG_PRINTF("[CMD] mbb status=0x%04X\r\n", status);
+
+    processing = false;
+    return 1;
+}
+
+/*
+ * mtb?: piezo sweep + IMU FIFO
+ */
+int cmd_mtb(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    uint8_t session = cmd_next_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 = piezo_measure_start_session();
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        status = piezo_measure_sweep();
+    }
+
+    if (status == ECHO_STATUS_OK)
+    {
+        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
+        {
+            cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(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;
+        }
+    }
+
+    cmd_send_response_rim(tx_rim_samples, rim_count);
+
+    piezo_power_off();
+    power_button_suspend(false);
+    cmd_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
+ */
+int cmd_mcf(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    const piezo_config_t *cfg = piezo_config_get();
+    cmd_send_response_piezo_config("rcf:", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples);
+    return 1;
+}
+
+/*
+ * mcs?: piezo 측정 파라미터 쓰기
+ */
+int cmd_mcs(const uint8_t *data, uint8_t data_len)
+{
+    piezo_config_t cfg;
+
+    if (data_len < 10U)
+    {
+        cmd_send_response_piezo_config("rcs:", 0xFFFF, 0U, 0U, 0U, 0U);
+        return 1;
+    }
+
+    cmd_get_data_u16_be(data, data_len, 0, &cfg.freq);
+    cmd_get_data_u16_be(data, data_len, 1, &cfg.cycles);
+    cmd_get_data_u16_be(data, data_len, 2, &cfg.avg);
+    cmd_get_data_u16_be(data, data_len, 3, &cfg.delay_us);
+    cmd_get_data_u16_be(data, data_len, 4, &cfg.samples);
+
+    if (!piezo_config_validate(&cfg))
+    {
+        cmd_send_response_piezo_config("rcs:", 0xFFFF, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
+        return 1;
+    }
+
+    int err = piezo_config_set(&cfg);
+    if (err)
+    {
+        cmd_send_response_piezo_config("rcs:", 0xFFFD, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
+        return 1;
+    }
+
+    const piezo_config_t *saved_cfg = piezo_config_get();
+    cmd_send_response_piezo_config("rcs:", saved_cfg->freq, saved_cfg->cycles, saved_cfg->avg, saved_cfg->delay_us, saved_cfg->samples);
+    return 1;
+}
+
+/* mls? → LED 상태 변경 → rls: + state echo back
+ * 파라미터: [state(2B LE)] — led_state_t enum 값
+ *   0=OFF, 4=DETACH_WARNING, 5=ALIGN_SEARCHING, 6=ALIGN_COMPLETE, 7=ERROR
+ * 에러 응답: 0xFFFF=파라미터 없음, 0xFFFE=범위 초과 */

src/command/handlers/cmd_piezo.h

@@ -0,0 +1,21 @@
+/*******************************************************************************
+ * @file    cmd_piezo.h
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#ifndef CMD_PIEZO_H__
+#define CMD_PIEZO_H__
+
+#include <stdint.h>
+
+int cmd_mpa(const uint8_t *data, uint8_t data_len);
+int cmd_mpb(const uint8_t *data, uint8_t data_len);
+int cmd_mpc(const uint8_t *data, uint8_t data_len);
+int cmd_mec(const uint8_t *data, uint8_t data_len);
+int cmd_mad(const uint8_t *data, uint8_t data_len);
+int cmd_maa(const uint8_t *data, uint8_t data_len);
+int cmd_mbb(const uint8_t *data, uint8_t data_len);
+int cmd_mtb(const uint8_t *data, uint8_t data_len);
+int cmd_mcf(const uint8_t *data, uint8_t data_len);
+int cmd_mcs(const uint8_t *data, uint8_t data_len);
+
+#endif /* CMD_PIEZO_H__ */

src/command/handlers/cmd_sensor.c

@@ -0,0 +1,123 @@
+/*******************************************************************************
+ * @file    cmd_sensor.c
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#include <zephyr/sys/util.h>
+#include <limits.h>
+
+#include "cmd_common.h"
+#include "main.h"
+#include "debug_print.h"
+#include "battery_adc.h"
+#include "imu_i2c.h"
+#include "led_control.h"
+#include "cmd_sensor.h"
+
+int cmd_msn(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    int mv = battery_read_mv();
+    if (mv < 0)
+    {
+        mv = 0;
+    }
+
+    cmd_send_response_u16("rsn:", (uint16_t)mv);
+    return 1;
+}
+
+int cmd_msp(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    int16_t accel[3], gyro[3];
+
+    int ret = imu_read(accel, gyro);
+    if (ret != 0)
+    {
+        cmd_send_response_u16("rsp:", 0xFFFF);
+        DBG_PRINTF("[CMD] msp: FAIL (imu_read ret=%d) -> rsp: 0xFFFF\r\n", ret);
+        return 1;
+    }
+
+    cmd_send_response_imu(accel, gyro);
+    return 1;
+}
+
+/* mst? -> IMU internal temperature -> rso: + temperature (degC x 100, BE)
+ * Error response: 0xFFFF = IMU temperature read failed. */
+int cmd_mst(const uint8_t *data, uint8_t data_len)
+{
+    ARG_UNUSED(data);
+    ARG_UNUSED(data_len);
+
+    power_button_suspend(true);
+
+    int16_t t_cdeg = INT16_MIN;
+    int ret = imu_read_temperature_cdeg(&t_cdeg);
+
+    power_button_suspend(false);
+
+    if ((ret != 0) || (t_cdeg == INT16_MIN))
+    {
+        cmd_send_response_u16("rso:", 0xFFFF);
+        DBG_PRINTF("[CMD] mst: imu temp read fail ret=%d\r\n", ret);
+        return 1;
+    }
+
+    /* 음수 온도도 2's complement로 그대로 전송 (앱이 int16로 해석) */
+    cmd_send_response_u16("rso:", (uint16_t)t_cdeg);
+    DBG_PRINTF("[CMD] mst -> %d.%02d C\r\n", t_cdeg / 100, (t_cdeg < 0 ? -t_cdeg : t_cdeg) % 100);
+    return 1;
+}
+
+/*
+ * 테스트용
+ * mpa?: piezo TX/RX 전원 레일만 켬
+ */
+int cmd_mls(const uint8_t *data, uint8_t data_len)
+{
+    /* 파라미터 부족: 에러 코드 0xFFFF 에코 */
+    if (data_len < 2)
+    {
+        cmd_send_response_u16("rls:", 0xFFFF);
+        DBG_PRINTF("[CMD] mls: no data\r\n");
+        return 1;
+    }
+
+    /* state 필드: 2바이트 big-endian */
+    uint16_t state = ((uint16_t)data[0] << 8) | (uint16_t)data[1];
+
+    /* 범위 초과: 에러 코드 0xFFFE 에코 */
+    if (state > LED_STATE_ERROR)
+    {
+        cmd_send_response_u16("rls:", 0xFFFE);
+        DBG_PRINTF("[CMD] mls: invalid state %d\r\n", state);
+        return 1;
+    }
+
+    led_set_state((led_state_t)state);
+
+    /* 정렬 LED 상태와 보호 판정 정책 연결
+     * - ALIGN_SEARCHING/ALIGN_COMPLETE 동안 보호 판정 차단
+     * - 다른 LED 상태에서는 기존 조건에 따라 보호 판정 재개
+     */
+    battery_protection_set_alignment_mode((state == LED_STATE_ALIGN_SEARCHING) ||
+                                          (state == LED_STATE_ALIGN_COMPLETE));
+
+    /* OFF 상태에서는 정렬/측정용 FIFO를 함께 정리 */
+    if (state == LED_STATE_OFF)
+    {
+        (void)imu_fifo_stop();
+    }
+
+    cmd_send_response_u16("rls:", state);
+    return 1;
+}
+
+/*==============================================================================
+ * 커맨드 테이블
+ *============================================================================*/

src/command/handlers/cmd_sensor.h

@@ -0,0 +1,15 @@
+/*******************************************************************************
+ * @file    cmd_sensor.h
+ * @brief   BLE command handlers
+ ******************************************************************************/
+#ifndef CMD_SENSOR_H__
+#define CMD_SENSOR_H__
+
+#include <stdint.h>
+
+int cmd_msn(const uint8_t *data, uint8_t data_len);
+int cmd_msp(const uint8_t *data, uint8_t data_len);
+int cmd_mst(const uint8_t *data, uint8_t data_len);
+int cmd_mls(const uint8_t *data, uint8_t data_len);
+
+#endif /* CMD_SENSOR_H__ */

src/command/parser.c

@@ -0,0 +1,88 @@
+/*******************************************************************************
+ * @file    parser.c
+ * @brief   BLE command parser
+ *
+ * Packet format : [TAG 4B] [DATA NB] [CRC16 2B]
+ ******************************************************************************/
+#include <stdint.h>
+#include <string.h>
+
+#include "parser.h"
+#include "cmd_common.h"
+#include "cmd_table.h"
+#include "debug_print.h"
+
+/*==============================================================================
+ * 파서 엔트리
+ *============================================================================*/
+int ble_cmd_dispatch(const uint8_t *buf, uint16_t len)
+{
+    DBG_CORE("[PARSER] enter len=%u\r\n", len);
+    DBG_CORE("[CMD] RX len=%u\r\n", len);
+
+    char raw_tag[5] = { '?', '?', '?', '?', '\0' };
+    for (uint16_t i = 0; i < len && i < 4U; i++)
+    {
+        raw_tag[i] = (char)buf[i];
+    }
+
+    if (len < 7U)
+    {
+        DBG_ERR("[CMD] Too short (%u)\r\n", len);
+        if (cmd_send_response_tag_echo("rxs:", raw_tag) != 0)
+        {
+            DBG_ERR("[CMD] rxs tx failed\r\n");
+        }
+        return -1;
+    }
+
+    uint16_t calc_crc = cmd_crc16_compute(buf, len - 2U);
+    uint16_t recv_crc = (uint16_t)(buf[len - 2U]) | ((uint16_t)(buf[len - 1U]) << 8);
+
+    if (calc_crc != recv_crc)
+    {
+        DBG_ERR("[CMD] CRC fail tag=%s calc=0x%04X recv=0x%04X\r\n", raw_tag, calc_crc, recv_crc);
+        if (cmd_send_response_tag_echo("rxc:", raw_tag) != 0)
+        {
+            DBG_ERR("[CMD] rxc tx failed\r\n");
+        }
+        return -1;
+    }
+
+    char tag[5] =
+    {
+        cmd_ascii_to_lower((char)buf[0]),
+        cmd_ascii_to_lower((char)buf[1]),
+        cmd_ascii_to_lower((char)buf[2]),
+        cmd_ascii_to_lower((char)buf[3]),
+        '\0'
+    };
+
+    const uint8_t *data = buf + 4;
+    uint8_t data_len = (uint8_t)(len - 4U - 2U);
+
+    for (int i = 0; i < cmd_table_size; i++)
+    {
+        if (memcmp(tag, cmd_table[i].tag, 4) == 0)
+        {
+            if (cmd_table[i].handler == NULL)
+            {
+                DBG_ERR("[CMD] Null handler: %s\r\n", cmd_table[i].tag);
+                if (cmd_send_response_tag_echo("rxn:", raw_tag) != 0)
+                {
+                    DBG_ERR("[CMD] rxn tx failed\r\n");
+                }
+                return -1;
+            }
+
+            return cmd_table[i].handler(data, data_len);
+        }
+    }
+
+    DBG_ERR("[CMD] Unknown: raw=%s normalized=%s\r\n", raw_tag, tag);
+    if (cmd_send_response_tag_echo("rxx:", raw_tag) != 0)
+    {
+        DBG_ERR("[CMD] rxx tx failed\r\n");
+    }
+    return 0;
+}