fix(posture): rsp: 응답 (msp 명령 응답) 파싱 추가 — 자세 분류 작동

증상: PiezoMonitoring 진입 시 자세 아이콘이 항상 ?_ (UNKNOWN) 만 뜸.

원인: sendImuQuery() 가 보내는 msp 명령은 rsp: 응답을 받는데, 기존 코드는
      rim: (mtb alignment 응답) 만 imuCollector.parseRim() 으로 처리.
      rsp: 는 onImuReceived callback 으로만 가는데 어디서도 set 안 됨
      (dead callback) → 자세 분류기에 sample 절대 도달 못 함 → UNKNOWN.

수정:
  1) ImuPacketCollector.parseRsp() 신규
     rsp: format spec 미확정이라 defensive parsing 으로 두 변형 시도:
     - 변형 1 (multi-sample): rim: 와 동일 [tag 4B][num 2B][N×12B][crc 2B]
     - 변형 2 (single sample): [tag 4B][12B sample][crc 2B] = 18B
     실패 시 hex dump 로그 출력 (다음 디버깅용)
     성공 시 imuCollector.onComplete 정상 호출 → 분류기 작동.

  2) BleManager rsp: 핸들러
     - parseRsp() 라우팅
     - rx 로그에 hex 추가 (실제 format 확인용)

기존 rim: 처리는 그대로 (alignment 모드 영향 없음).

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
2026-06-23 14:06:40 +09:00
parent cf476971a5
commit 1774681a18
2 changed files with 77 additions and 3 deletions
@@ -1209,9 +1209,10 @@ class BleManager private constructor(private val context: Context) {
piezoCollector.addPacket(data)
}
"rsp:" -> {
if (data.size >= 16) {
debugLogger.rx("rsp", data.size, "IMU data")
}
// msp 명령 응답 — IMU 데이터. format 불명확이라 defensive parsing.
val hex = data.take(kotlin.math.min(data.size, 24)).joinToString(" ") { "%02X".format(it) }
debugLogger.rx("rsp", data.size, "IMU data hex=$hex")
imuCollector.parseRsp(data)
onImuReceived?.invoke(data)
}
"rim:" -> {
@@ -50,6 +50,79 @@ class ImuPacketCollector {
samples = emptyList()
}
/**
* rsp: 패킷 처리 — msp 명령 응답.
* Format 가정 (firmware spec 미확정 — defensive parsing):
* 변형 1 (multi-sample, rim: 와 동일): [tag 4B][num 2B BE][N×12B sample][crc 2B]
* 변형 2 (single-sample): [tag 4B][12B sample][crc 2B] = 18B
*
* 시도 순서: 변형 1 → 실패 시 변형 2 → 둘 다 실패 시 hex log + 무시.
*/
fun parseRsp(data: ByteArray) {
if (data.size < 4 + CRC_BYTES) {
onLog?.invoke("rsp: too short (${data.size}B)")
return
}
// 변형 1 시도 (rim: 와 동일 format)
val num1 = readBe16(data, 4)
val expected1 = HEADER_BYTES + num1 * SAMPLE_SIZE_BYTES + CRC_BYTES
if (num1 in 1..15 && data.size >= expected1) {
parseMultiSample(data, num1)
return
}
// 변형 2 시도 (single sample, 4B tag + 12B sample + 2B crc = 18B)
if (data.size >= 18) {
val parsed = ArrayList<ImuSample>(1)
val off = 4
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
val first = parsed[0]
onLog?.invoke("rsp: 1 sample (single fmt, Az=${"%.3f".format(first.az)}g)")
onComplete?.invoke(parsed)
return
}
// 둘 다 실패 — hex dump
val hex = data.take(kotlin.math.min(data.size, 24)).joinToString(" ") { "%02X".format(it) }
onLog?.invoke("rsp: unknown format (${data.size}B) hex=$hex")
}
private fun parseMultiSample(data: ByteArray, numSamples: Int) {
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("rsp: $numSamples samples (multi fmt, last Az=${"%.3f".format(parsed.last().az)}g)")
onComplete?.invoke(parsed)
}
/** rim: 패킷 한 번 받으면 호출. 188B 전체 ByteArray. */
fun parseRim(data: ByteArray) {
if (data.size < HEADER_BYTES + CRC_BYTES) {