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>
This commit is contained in:
@@ -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()
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user