diff --git a/SulTee/SulTee/BLEPackets.swift b/SulTee/SulTee/BLEPackets.swift index e92de49..592a549 100644 --- a/SulTee/SulTee/BLEPackets.swift +++ b/SulTee/SulTee/BLEPackets.swift @@ -87,9 +87,14 @@ enum BLEPackets { // MARK: - Ranging notification parser // - // HAL firmware format (8 bytes, 2 fixed anchors): - // [0-3] Int32 LE front anchor range mm (id=0) - // [4-7] Int32 LE back anchor range mm (id=1) + // HAL firmware format v3.3 (8 bytes, 2 fixed anchors): + // [0-3] Int32 LE front anchor range mm (id=0) + // [4-7] Int32 LE back anchor range mm (id=1) + // + // HAL firmware format v3.4 (12 bytes, adds signal quality): + // [0-3] Int32 LE front anchor range mm (id=0) + // [4-7] Int32 LE back anchor range mm (id=1) + // [8-11] Float32 LE best_rssi dBm (shared; assigned to both anchors) // // Legacy multi-anchor format (future): // [0] Uint8 anchor count N @@ -102,15 +107,18 @@ enum BLEPackets { static func parseRanging(_ data: Data) -> [AnchorInfo] { let now = Date() - // HAL two-anchor format: exactly 8 bytes, two Int32 LE range values - if data.count == 8 { + // HAL two-anchor format: 8 bytes (v3.3) or 12 bytes (v3.4 + RSSI float) + if data.count == 8 || data.count == 12 { let frontMM = data.readInt32LE(at: 0) let backMM = data.readInt32LE(at: 4) + let rssi: Double? = data.count == 12 + ? Double(data.readFloat32LE(at: 8)) + : nil return [ AnchorInfo(id: 0, rangeMetres: Double(frontMM) / 1000.0, - rssiDBm: nil, ageMs: 0, receivedAt: now), + rssiDBm: rssi, ageMs: 0, receivedAt: now), AnchorInfo(id: 1, rangeMetres: Double(backMM) / 1000.0, - rssiDBm: nil, ageMs: 0, receivedAt: now) + rssiDBm: rssi, ageMs: 0, receivedAt: now) ] } @@ -182,5 +190,12 @@ private extension Data { func readUInt16LE(at offset: Int) -> UInt16 { UInt16(self[offset]) | (UInt16(self[offset + 1]) << 8) } + func readFloat32LE(at offset: Int) -> Float { + let bits = UInt32(self[offset]) + | (UInt32(self[offset + 1]) << 8) + | (UInt32(self[offset + 2]) << 16) + | (UInt32(self[offset + 3]) << 24) + return Float(bitPattern: bits) + } }