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:
@@ -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) {
|
||||
|
||||
Reference in New Issue
Block a user