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
+    }
+}
@@ -0,0 +1,91 @@
+package com.example.medilightv2android.managers
+
+import com.example.medilightv2android.ble.ImuSample
+import kotlin.math.abs
+import kotlin.math.atan2
+import kotlin.math.sqrt
+
+/**
+ * IMU 샘플 묶음(한 mtb 응답 = 15샘플)에서 자세/안정성 지표 추출.
+ *
+ * 가정: VBT 센서의 Accel z축이 환자 몸 바깥 방향(센서 면 → 위).
+ * 평평하게 누운 환자(Supine)에 부착 시 Az ≈ -1g (또는 +1g, 부착 면에 따라).
+ *
+ * 향후 임상 데이터로 calibration 필요.
+ */
+object ImuAnalysis {
+
+    /** 한 응답 안의 모든 샘플 평균 Accel vector */
+    fun meanAccel(samples: List<ImuSample>): Triple<Float, Float, Float>? {
+        if (samples.isEmpty()) return null
+        var sx = 0f; var sy = 0f; var sz = 0f
+        for (s in samples) { sx += s.ax; sy += s.ay; sz += s.az }
+        val n = samples.size
+        return Triple(sx / n, sy / n, sz / n)
+    }
+
+    /** 한 응답 안의 모든 샘플 평균 Gyro vector */
+    fun meanGyro(samples: List<ImuSample>): Triple<Float, Float, Float>? {
+        if (samples.isEmpty()) return null
+        var sx = 0f; var sy = 0f; var sz = 0f
+        for (s in samples) { sx += s.gx; sy += s.gy; sz += s.gz }
+        val n = samples.size
+        return Triple(sx / n, sy / n, sz / n)
+    }
+
+    /**
+     * 흔들림 강도 — Gyro 표준편차의 RMS. 정지 시 ≈ 0~5 dps, 흔드는 중 ≈ 50+ dps.
+     */
+    fun shakeRmsDps(samples: List<ImuSample>): Float {
+        if (samples.size < 2) return 0f
+        val mean = meanGyro(samples) ?: return 0f
+        var sum = 0f
+        for (s in samples) {
+            val dx = s.gx - mean.first
+            val dy = s.gy - mean.second
+            val dz = s.gz - mean.third
+            sum += dx * dx + dy * dy + dz * dz
+        }
+        return sqrt(sum / samples.size)
+    }
+
+    /**
+     * 자세 추정 (간이 — Accel만 사용).
+     * 평균 Accel vector의 방향 → 어느 축이 중력에 가까운지 판정.
+     *
+     * 반환: "Supine" / "Sitting" / "Standing" / "Tilted" / "Moving"
+     *
+     * 임시 기준 (임상 데이터로 보정 필요):
+     *  - |Az| 가장 큼 + Az > 0.7g: 환자 몸 누움 (Supine, 센서면 위 향함)
+     *  - |Az| 가장 큼 + Az < -0.7g: 센서 뒤집힘
+     *  - |Ay| 가장 큼: Sitting (몸 세움)
+     *  - |Ax| 가장 큼: Standing 또는 옆 기울기
+     *  - 기타: Tilted
+     *  - shakeRmsDps > 30: Moving (자세 판정 불가)
+     */
+    fun estimatePosture(samples: List<ImuSample>): String {
+        if (samples.isEmpty()) return "—"
+        if (shakeRmsDps(samples) > 30f) return "Moving"
+        val (ax, ay, az) = meanAccel(samples) ?: return "—"
+        val axAbs = abs(ax); val ayAbs = abs(ay); val azAbs = abs(az)
+        return when {
+            azAbs >= axAbs && azAbs >= ayAbs && azAbs > 0.7f ->
+                if (az > 0) "Supine" else "Supine (flip)"
+            ayAbs >= axAbs && ayAbs > 0.6f -> "Sitting"
+            axAbs > 0.6f -> "Standing"
+            else -> "Tilted"
+        }
+    }
+
+    /**
+     * Tilt 각도 (도) — 센서 z축이 중력에 대해 얼마나 기울었는지.
+     * 0° = z축이 정확히 중력 방향, 90° = z축이 수평.
+     */
+    fun tiltDegrees(samples: List<ImuSample>): Float {
+        val (ax, ay, az) = meanAccel(samples) ?: return 0f
+        val mag = sqrt(ax * ax + ay * ay + az * az)
+        if (mag < 0.1f) return 0f  // 자유낙하/이상치
+        val tiltRad = atan2(sqrt(ax * ax + ay * ay), abs(az))
+        return (tiltRad * 180.0 / Math.PI).toFloat()
+    }
+}
@@ -0,0 +1,66 @@
+package com.example.medilightv2android.services
+
+import com.example.medilightv2android.ble.ImuSample
+import java.io.File
+import java.text.SimpleDateFormat
+import java.util.Date
+import java.util.Locale
+
+/**
+ * IMU 샘플을 CSV로 저장. 임상 R&D 모드 한정.
+ *
+ * 파일 위치 (임상 모드일 때):
+ *   Downloads/VesiScan_Sessions/{folder}/imu.csv
+ *
+ * 한 번의 mtb? 응답에서 15 샘플 → CSV 15행. scan_id로 같은 측정의 piezo adc.csv와 매칭 가능.
+ *
+ * Sensitivity:
+ *   ax/ay/az: g (Accel ±4g, 1 LSB = 1/8192 g)
+ *   gx/gy/gz: dps (Gyro ±500 dps, 1 LSB = 1/65.536 dps)
+ *
+ * Sample 순서: 한 응답 안에서 oldest→newest. sample_idx 컬럼으로 명시.
+ */
+object ImuCsvLogger {
+
+    private val timeFmt = SimpleDateFormat("HH:mm:ss.SSS", Locale.US)
+
+    /**
+     * 임상 모드 한정. ClinicalSessionStore.currentLogDir()가 있을 때만 저장.
+     * samples를 oldest→newest 순서로 그대로 기록.
+     */
+    fun log(samples: List<ImuSample>) {
+        val dir = ClinicalSessionStore.currentLogDir() ?: return
+        if (samples.isEmpty()) return
+        try {
+            dir.mkdirs()
+            val file = File(dir, "imu.csv")
+            val isNew = !file.exists() || file.length() == 0L
+            val scanId = AdcCsvLogger.lastScanId   // piezo adc.csv와 매칭용
+            val timestamp = timeFmt.format(Date())
+            val clinicalLabel = ClinicalSessionStore.currentLabel()
+            val fwVersion = com.example.medilightv2android.ble.BleManager.current?.firmwareVersion?.value ?: ""
+
+            file.bufferedWriter().use { _ -> }  // ensure exists
+            java.io.FileWriter(file, !isNew).buffered().use { writer ->
+                if (isNew) {
+                    writer.write("scan_id,timestamp,clinical_label,firmware_version,sample_idx,ax_g,ay_g,az_g,gx_dps,gy_dps,gz_dps")
+                    writer.newLine()
+                }
+                for ((idx, s) in samples.withIndex()) {
+                    writer.write(buildString {
+                        append("$scanId,$timestamp,$clinicalLabel,$fwVersion,$idx,")
+                        append("%.4f".format(s.ax)); append(",")
+                        append("%.4f".format(s.ay)); append(",")
+                        append("%.4f".format(s.az)); append(",")
+                        append("%.4f".format(s.gx)); append(",")
+                        append("%.4f".format(s.gy)); append(",")
+                        append("%.4f".format(s.gz))
+                    })
+                    writer.newLine()
+                }
+            }
+        } catch (_: Exception) {
+            // 로그는 측정 자체를 막으면 안 됨 — silent fail
+        }
+    }
+}
@@ -86,6 +86,8 @@ fun PlacementGuideView(appState: AppState) {
    val greenHoldMs = 7000L  // GREEN 후 최소 7초 유지
    var greenFailCount by remember { mutableIntStateOf(0) }  // GREEN 이탈 디바운스
    var lastChannelData by remember { mutableStateOf<List<PiezoChannelData>>(emptyList()) }
+    // 최근 IMU 샘플 (15개, ODR 50Hz, ~300ms 분량). 자세 판정/흔들림 감지/임상 로그용.
+    var lastImuSamples by remember { mutableStateOf<List<com.example.medilightv2android.ble.ImuSample>>(emptyList()) }
    var lastWallResults by remember { mutableStateOf<List<Pair<Int, Int>?>>(List(6) { null }) }
    var debounceCount by remember { mutableIntStateOf(0) }
    var lastGuidePhase by remember { mutableStateOf(PlacementPhase.VERTICAL) }
@@ -244,6 +246,27 @@ fun PlacementGuideView(appState: AppState) {
                            .padding(horizontal = 6.dp, vertical = 1.dp)
                    )
                }
+                // IMU 정보 (Dev 모드 한정) — 자세 + 흔들림 RMS
+                if (appState.isDevMode && lastImuSamples.isNotEmpty()) {
+                    val posture = remember(lastImuSamples) {
+                        com.example.medilightv2android.managers.ImuAnalysis.estimatePosture(lastImuSamples)
+                    }
+                    val shake = remember(lastImuSamples) {
+                        com.example.medilightv2android.managers.ImuAnalysis.shakeRmsDps(lastImuSamples)
+                    }
+                    val tilt = remember(lastImuSamples) {
+                        com.example.medilightv2android.managers.ImuAnalysis.tiltDegrees(lastImuSamples)
+                    }
+                    Text(
+                        "IMU: $posture · tilt=${"%.0f".format(tilt)}° · shake=${"%.1f".format(shake)}dps",
+                        fontSize = 10.sp,
+                        fontWeight = FontWeight.SemiBold,
+                        color = if (shake > 30f) Color.Red else MlSecondaryText,
+                        modifier = Modifier
+                            .background(Color.Gray.copy(alpha = 0.15f), RoundedCornerShape(4.dp))
+                            .padding(horizontal = 6.dp, vertical = 1.dp)
+                    )
+                }
            }

            // 타이틀 — 28sp Bold (Hint와의 강조 균형. 이전 38sp ExtraBold는 화면을 압도)
@@ -827,7 +850,17 @@ fun PlacementGuideView(appState: AppState) {
                }
                } // handler.post
                bleManager.piezoCollector.onLog = { msg -> Log.d("PlacementGuide", msg) }
-                if (!bleManager.sendAllChannels()) {
+                // IMU 콜백 등록 — rim: 응답이 reb×6 + raa 이후 도착. 임상 모드면 imu.csv에 자동 저장.
+                bleManager.imuCollector.onComplete = { imuSamples ->
+                    handler.post {
+                        lastImuSamples = imuSamples
+                        if (com.example.medilightv2android.services.ClinicalSessionStore.currentSession != null) {
+                            com.example.medilightv2android.services.ImuCsvLogger.log(imuSamples)
+                        }
+                    }
+                }
+                // mtb? — Piezo 6채널 + IMU 동시 캡처. 응답: reb×6 + raa + rim
+                if (!bleManager.sendMtb()) {
                    // Throttle됨 — isScanning 리셋 (deadlock 방지)
                    isScanning = false
                }