feat: Sensor Alignment에 IMU 통합 (mtb? + rim: 정식 처리)

PPT 'VesiScan-Basic IMU FW 진행사항_260521' 사양 기반 구현. 실측 1회 (2026-05-27 12:24, 평평한 책상)로 endian/필드 순서/시간 순서 확정 후 정식 적용. 확정 사양 (실측): - rim: 패킷 188B = [tag 4B "rim:"] [num 2B BE] [data 180B = 15 samples × 12B] [crc 2B] - 1 sample = 12B: Ax 2B + Ay 2B + Az 2B + Gx 2B + Gy 2B + Gz 2B (모두 BE int16) - Accel ±4g sensitivity: /8192 g/LSB - Gyro ±500 dps sensitivity: /65.536 dps/LSB - 샘플 순서: samples[0] = oldest (FIFO 순) 신규 파일: - ble/ImuPacketCollector.kt — rim: 파서. ImuSample data class + 15-sample list 콜백 - services/ImuCsvLogger.kt — Clinical 모드 한정 imu.csv 자동 저장 컬럼: scan_id, timestamp, clinical_label, firmware_version, sample_idx, ax_g, ay_g, az_g, gx_dps, gy_dps, gz_dps - managers/ImuAnalysis.kt — 자세 추정/Tilt/흔들림 RMS 유틸 (간이, 임상 보정 필요) 수정: - BleManager: imuCollector 필드 + sendMtb() 정식 명령 (canSendMaa() 게이트 maa와 공유. piezo + IMU 동시 캡처) - BleManager: rim: 핸들러 정식화 (imuCollector.parseRim 위임) - PlacementGuideView: sendAllChannels → sendMtb 교체, imuCollector.onComplete 등록 Clinical 모드일 때 ImuCsvLogger.log() 자동 호출 Dev 모드 한정 상단 칩에 "IMU: Supine · tilt=N° · shake=N.Ndps" 표시 데이터 흐름: performScan → sendMtb? → 응답: 1) reb × 6 (Piezo, 기존 처리) 2) raa (Piezo end, 기존 onMultiChannelComplete 트리거) 3) rim (IMU, 신규 imuCollector.onComplete → lastImuSamples 갱신 + imu.csv) 분석: - ImuAnalysis.estimatePosture: Accel 평균으로 Supine/Sitting/Standing/Tilted/Moving 판정 - shakeRmsDps > 30 → "Moving" (자세 판정 보류) - tiltDegrees: z축 vs 중력 각도 Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-05-27 13:19:20 +09:00
parent 020a45ba92
commit ec3be6fa5d
5 changed files with 313 additions and 1 deletions
@@ -392,6 +392,8 @@ class BleManager private constructor(private val context: Context) {

    // Piezo Commands
    val piezoCollector = PiezoPacketCollector()
+    // IMU rim: 응답 파서. mtb? 명령 시 채워짐. PlacementGuide/Clinical에서 콜백 등록.
+    val imuCollector = ImuPacketCollector()

    // ── Piezo Commands (테스터 exe와 동일 포맷: 파라미터=BE, 빈 파라미터=공백) ──

@@ -415,6 +417,21 @@ class BleManager private constructor(private val context: Context) {
        return true
    }

+    /**
+     * 정렬 모드 측정 (mtb?) — Piezo 6채널 + IMU 동시 캡처.
+     * 응답: reb: × 6 + raa: + rim: (각각 piezoCollector / imuCollector가 처리)
+     *
+     * canSendMaa() 게이트는 maa와 공유 (lastMaaSentMs 같이 사용).
+     */
+    fun sendMtb(): Boolean {
+        if (!canSendMaa("sendMtb")) return false
+        lastMaaSentMs = System.currentTimeMillis()
+        piezoCollector.startMultiChannel(6)
+        imuCollector.reset()
+        sendRaw(CRC16.buildCommandASCII("mtb", " "))
+        return true
+    }
+
    // ── NIRS Commands ──

    fun sendNirsPowerOn() {
@@ -1013,6 +1030,11 @@ class BleManager private constructor(private val context: Context) {
                }
                onImuReceived?.invoke(data)
            }
+            "rim:" -> {
+                val num = if (data.size >= 6) ((data[4].toInt() and 0xFF) shl 8) or (data[5].toInt() and 0xFF) else 0
+                debugLogger.rx("rim", data.size, "IMU ${num} samples")
+                imuCollector.parseRim(data)
+            }
            else -> {
                val hex = data.take(8).joinToString(" ") { "%02X".format(it) }
                debugLogger.rx("???", data.size, "unknown prefix='$prefix' hex=$hex")
@@ -0,0 +1,100 @@
+package com.example.medilightv2android.ble
+
+/**
+ * IMU 6축 raw 샘플 (Accel xyz + Gyro xyz).
+ *
+ * FW VBTFW0116+ rim: 응답 한 패킷에 15 샘플 (ODR 50Hz → 약 300ms 분량).
+ * Sensitivity:
+ *   - Accel ±4g: 1 LSB = 1/8192 g
+ *   - Gyro ±500 dps: 1 LSB = 1/65.536 dps
+ */
+data class ImuSample(
+    val ax: Float, val ay: Float, val az: Float,   // g 단위 (중력 가속도)
+    val gx: Float, val gy: Float, val gz: Float    // dps 단위 (degrees/sec)
+) {
+    /** 전체 가속도 크기 — 정지 시 ≈ 1g */
+    val accelMagnitude: Float get() = kotlin.math.sqrt(ax * ax + ay * ay + az * az)
+    /** Gyro 전체 크기 — 정지 시 ≈ 0 dps */
+    val gyroMagnitude: Float get() = kotlin.math.sqrt(gx * gx + gy * gy + gz * gz)
+}
+
+/**
+ * rim: 패킷 파서.
+ *
+ * 패킷 구조 (총 188B):
+ *   [tag 4B "rim:"] [num_sample 2B BE] [imu_raw_data 180B] [crc 2B]
+ *
+ * imu_raw_data = 15 samples × 12B/sample, 모두 BE int16:
+ *   [Ax 2B] [Ay 2B] [Az 2B] [Gx 2B] [Gy 2B] [Gz 2B]
+ *
+ * 샘플 시간 순서: samples[0] = oldest (FIFO 순), samples[N-1] = newest.
+ * 실측 검증 완료 (Az ≈ +1g 평평 자세, 2026-05-27).
+ */
+class ImuPacketCollector {
+
+    var samples: List<ImuSample> = emptyList()
+        private set
+
+    var onComplete: ((List<ImuSample>) -> Unit)? = null
+    var onLog: ((String) -> Unit)? = null
+
+    companion object {
+        private const val ACCEL_LSB_PER_G = 8192f       // ±4g FSR
+        private const val GYRO_LSB_PER_DPS = 65.536f    // ±500 dps FSR
+        private const val SAMPLE_SIZE_BYTES = 12
+        private const val HEADER_BYTES = 6              // tag(4) + num(2)
+        private const val CRC_BYTES = 2
+    }
+
+    fun reset() {
+        samples = emptyList()
+    }
+
+    /** rim: 패킷 한 번 받으면 호출. 188B 전체 ByteArray. */
+    fun parseRim(data: ByteArray) {
+        if (data.size < HEADER_BYTES + CRC_BYTES) {
+            onLog?.invoke("rim: too short (${data.size}B)")
+            return
+        }
+        val numSamples = readBe16(data, 4)
+        val payloadBytes = data.size - HEADER_BYTES - CRC_BYTES
+        val expectedBytes = numSamples * SAMPLE_SIZE_BYTES
+        if (payloadBytes < expectedBytes) {
+            onLog?.invoke("rim: payload short ($payloadBytes < $expectedBytes for $numSamples samples)")
+            return
+        }
+        val parsed = ArrayList<ImuSample>(numSamples)
+        for (i in 0 until numSamples) {
+            val off = HEADER_BYTES + i * SAMPLE_SIZE_BYTES
+            val axRaw = readBe16Signed(data, off)
+            val ayRaw = readBe16Signed(data, off + 2)
+            val azRaw = readBe16Signed(data, off + 4)
+            val gxRaw = readBe16Signed(data, off + 6)
+            val gyRaw = readBe16Signed(data, off + 8)
+            val gzRaw = readBe16Signed(data, off + 10)
+            parsed.add(ImuSample(
+                ax = axRaw / ACCEL_LSB_PER_G,
+                ay = ayRaw / ACCEL_LSB_PER_G,
+                az = azRaw / ACCEL_LSB_PER_G,
+                gx = gxRaw / GYRO_LSB_PER_DPS,
+                gy = gyRaw / GYRO_LSB_PER_DPS,
+                gz = gzRaw / GYRO_LSB_PER_DPS,
+            ))
+        }
+        samples = parsed
+        onLog?.invoke("rim: $numSamples samples parsed (last: Az=${"%.3f".format(parsed.last().az)}g)")
+        onComplete?.invoke(parsed)
+    }
+
+    private fun readBe16(data: ByteArray, idx: Int): Int {
+        val hi = data[idx].toInt() and 0xFF
+        val lo = data[idx + 1].toInt() and 0xFF
+        return (hi shl 8) or lo
+    }
+
+    private fun readBe16Signed(data: ByteArray, idx: Int): Int {
+        var v = readBe16(data, idx)
+        if (v >= 0x8000) v -= 0x10000
+        return v
+    }
+}