IMU 측정, 커맨드 및 응답 추가

src/parser.c

@@ -16,6 +16,7 @@
 #include "ble_service.h"
 #include "battery_adc.h"
 #include "led_control.h"
+#include "imu_i2c.h"
 
 /*==============================================================================
  * CRC16 (CRC-CCITT, Nordic SDK 호환)
@@ -58,6 +59,34 @@ static void send_response_u16(const char *tag, uint16_t value)
     ble_data_send(buf, 8);
 }
 
+/*==============================================================================
+ * 응답 패킷 전송 (IMU)
+ *============================================================================*/
+
+/* TAG(4B) + int16×6 빅엔디안(12B) + CRC16(2B) = 18바이트 전송
+ * 기존 format_data() + dr_binary_tx_safe(buf, 8) 방식과 동일한 레이아웃 */
+static void send_response_imu(const int16_t accel[3], const int16_t gyro[3])
+{
+    uint8_t buf[18];
+
+    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 = dr_crc16_compute(buf, 16);
+    buf[16] = (uint8_t)(crc & 0xFF);
+    buf[17] = (uint8_t)(crc >> 8);
+
+    ble_data_send(buf, 18);
+}
+
 /*==============================================================================
  * 커맨드 핸들러
  *============================================================================*/
@@ -77,6 +106,22 @@ static int cmd_msn(const uint8_t *data, uint8_t data_len)
     return 1;
 }
 
+/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */
+static int cmd_msp(const uint8_t *data, uint8_t data_len)
+{
+    int16_t accel[3], gyro[3];
+
+    int ret = imu_read(accel, gyro);
+    if (ret != 0) {
+        send_response_u16("rsp:", 0xFFFF);
+        DBG_PRINTF("[CMD] msp: FAIL (imu_read ret=%d) -> rsp: 0xFFFF\r\n", ret);
+        return 1;
+    }
+
+    send_response_imu(accel, gyro);
+    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
@@ -121,6 +166,7 @@ static const cmd_entry_t cmd_table[] =
 {
     { "msn?", cmd_msn },
     { "mls?", cmd_mls },
+    { "msp?", cmd_msp },
 };
 
 #define CMD_TABLE_SIZE  (sizeof(cmd_table) / sizeof(cmd_table[0]))