sl-ios
9197371522
HAL v3.4 extends the range notify from 8→12 bytes: [0-3] int32 LE front_mm [4-7] int32 LE back_mm [8-11] float32 LE best_rssi dBm (NEW) Parser now handles both lengths (8=v3.3, 12=v3.4) by detecting packet size. RSSI is extracted as Double and assigned to both Front and Back anchors (shared signal quality); nil for v3.3. UI already conditionally renders rssiString (guard on Optional). Added Data.readFloat32LE(at:) helper using IEEE 754 bit-pattern reinterpretation (little-endian UInt32 → Float(bitPattern:)). Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
202 lines
8.2 KiB
Swift
202 lines
8.2 KiB
Swift
import Foundation
|
||
import CoreLocation
|
||
import CoreMotion
|
||
|
||
/// Builds BLE write packets in the exact binary format expected by the SaltyTag UWB firmware,
|
||
/// and parses incoming ranging notifications.
|
||
///
|
||
/// All multi-byte fields are little-endian.
|
||
enum BLEPackets {
|
||
|
||
// MARK: - GPS packet (20 bytes)
|
||
//
|
||
// [0-3] Int32 LE latitude × 1e7
|
||
// [4-7] Int32 LE longitude × 1e7
|
||
// [8-9] Int16 LE altitude × 10 (dm, clamped ±32767)
|
||
// [10-11] Uint16 LE speed × 100 (cm/s, clamped 0–65535)
|
||
// [12-13] Uint16 LE heading × 100 (0.01°, clamped 0–35999)
|
||
// [14] Uint8 accuracy × 10 (clamped 0–255)
|
||
// [15] Uint8 fix_type (0=mocked 1=2D 2=3D)
|
||
// [16-19] Uint32 LE timestamp lower 32 bits of ms since epoch
|
||
|
||
static func gpsPacket(from location: CLLocation) -> Data {
|
||
var buf = Data(count: 20)
|
||
|
||
let lat = Int32(clamping: Int64((location.coordinate.latitude * 1e7).rounded()))
|
||
let lon = Int32(clamping: Int64((location.coordinate.longitude * 1e7).rounded()))
|
||
let altDm = Int16(clamping: Int64((location.altitude * 10).rounded()))
|
||
let speedCms = UInt16(clamping: Int64(max(0, location.speed * 100).rounded()))
|
||
let course = location.course >= 0 ? location.course : 0
|
||
let hdg = UInt16(clamping: Int64((course * 100).rounded()) % 36000)
|
||
let acc = UInt8(clamping: Int64(max(0, location.horizontalAccuracy * 10).rounded()))
|
||
let fixType: UInt8 = location.horizontalAccuracy > 0 ? 2 : 1
|
||
let tsMsLow = UInt32(UInt64(location.timestamp.timeIntervalSince1970 * 1000) & 0xFFFFFFFF)
|
||
|
||
buf.writeInt32LE(lat, at: 0)
|
||
buf.writeInt32LE(lon, at: 4)
|
||
buf.writeInt16LE(altDm, at: 8)
|
||
buf.writeUInt16LE(speedCms, at: 10)
|
||
buf.writeUInt16LE(hdg, at: 12)
|
||
buf[14] = acc
|
||
buf[15] = fixType
|
||
buf.writeUInt32LE(tsMsLow, at: 16)
|
||
|
||
return buf
|
||
}
|
||
|
||
// MARK: - IMU packet (22 bytes)
|
||
//
|
||
// [0-1] Int16 LE accel X milli-g (m/s² already in g in CoreMotion → ×1000)
|
||
// [2-3] Int16 LE accel Y
|
||
// [4-5] Int16 LE accel Z
|
||
// [6-7] Int16 LE gyro X centi-deg/s (rad/s × 5729.578)
|
||
// [8-9] Int16 LE gyro Y
|
||
// [10-11] Int16 LE gyro Z
|
||
// [12-13] Int16 LE mag X μT
|
||
// [14-15] Int16 LE mag Y
|
||
// [16-17] Int16 LE mag Z
|
||
// [18-21] Uint32 LE timestamp lower 32 bits of ms since epoch
|
||
|
||
static func imuPacket(from motion: CMDeviceMotion) -> Data {
|
||
var buf = Data(count: 22)
|
||
|
||
// userAcceleration is already in g's (CoreMotion convention)
|
||
let ax = Int16(clamping: Int64((motion.userAcceleration.x * 1000).rounded()))
|
||
let ay = Int16(clamping: Int64((motion.userAcceleration.y * 1000).rounded()))
|
||
let az = Int16(clamping: Int64((motion.userAcceleration.z * 1000).rounded()))
|
||
|
||
// rotationRate is in rad/s; multiply by 5729.578 to get centi-deg/s
|
||
let gx = Int16(clamping: Int64((motion.rotationRate.x * 5729.578).rounded()))
|
||
let gy = Int16(clamping: Int64((motion.rotationRate.y * 5729.578).rounded()))
|
||
let gz = Int16(clamping: Int64((motion.rotationRate.z * 5729.578).rounded()))
|
||
|
||
// magneticField.field is in μT; pack directly as Int16
|
||
let mx = Int16(clamping: Int64(motion.magneticField.field.x.rounded()))
|
||
let my = Int16(clamping: Int64(motion.magneticField.field.y.rounded()))
|
||
let mz = Int16(clamping: Int64(motion.magneticField.field.z.rounded()))
|
||
|
||
let tsMsLow = UInt32(UInt64(Date().timeIntervalSince1970 * 1000) & 0xFFFFFFFF)
|
||
|
||
buf.writeInt16LE(ax, at: 0); buf.writeInt16LE(ay, at: 2); buf.writeInt16LE(az, at: 4)
|
||
buf.writeInt16LE(gx, at: 6); buf.writeInt16LE(gy, at: 8); buf.writeInt16LE(gz, at: 10)
|
||
buf.writeInt16LE(mx, at: 12); buf.writeInt16LE(my, at: 14); buf.writeInt16LE(mz, at: 16)
|
||
buf.writeUInt32LE(tsMsLow, at: 18)
|
||
|
||
return buf
|
||
}
|
||
|
||
// MARK: - Ranging notification parser
|
||
//
|
||
// 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
|
||
// Per anchor (9 bytes):
|
||
// [+0] Uint8 anchor index
|
||
// [+1-4] Int32 LE range mm
|
||
// [+5-6] Int16 LE RSSI × 10 (dBm × 10)
|
||
// [+7-8] Uint16LE age ms
|
||
|
||
static func parseRanging(_ data: Data) -> [AnchorInfo] {
|
||
let now = Date()
|
||
|
||
// 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: rssi, ageMs: 0, receivedAt: now),
|
||
AnchorInfo(id: 1, rangeMetres: Double(backMM) / 1000.0,
|
||
rssiDBm: rssi, ageMs: 0, receivedAt: now)
|
||
]
|
||
}
|
||
|
||
// Legacy multi-anchor format: [count][anchorID+rangeMM+RSSI+age] × N
|
||
guard data.count >= 1 else { return [] }
|
||
let count = Int(data[0])
|
||
var result: [AnchorInfo] = []
|
||
|
||
for i in 0..<count {
|
||
let base = 1 + i * 9
|
||
guard base + 9 <= data.count else { break }
|
||
|
||
let anchorID = data[base]
|
||
let rangeMM = data.readInt32LE(at: base + 1)
|
||
let rssiTimes10 = data.readInt16LE(at: base + 5)
|
||
let ageMs = data.readUInt16LE(at: base + 7)
|
||
|
||
result.append(AnchorInfo(
|
||
id: anchorID,
|
||
rangeMetres: Double(rangeMM) / 1000.0,
|
||
rssiDBm: Double(rssiTimes10) / 10.0,
|
||
ageMs: ageMs,
|
||
receivedAt: now
|
||
))
|
||
|
||
}
|
||
return result
|
||
}
|
||
}
|
||
|
||
// MARK: - Data helpers (little-endian read / write)
|
||
|
||
private extension Data {
|
||
mutating func writeInt16LE(_ value: Int16, at offset: Int) {
|
||
let v = UInt16(bitPattern: value)
|
||
self[offset] = UInt8(v & 0xFF)
|
||
self[offset + 1] = UInt8(v >> 8)
|
||
}
|
||
mutating func writeUInt16LE(_ value: UInt16, at offset: Int) {
|
||
self[offset] = UInt8(value & 0xFF)
|
||
self[offset + 1] = UInt8(value >> 8)
|
||
}
|
||
mutating func writeInt32LE(_ value: Int32, at offset: Int) {
|
||
let v = UInt32(bitPattern: value)
|
||
self[offset] = UInt8(v & 0xFF)
|
||
self[offset + 1] = UInt8((v >> 8) & 0xFF)
|
||
self[offset + 2] = UInt8((v >> 16) & 0xFF)
|
||
self[offset + 3] = UInt8((v >> 24) & 0xFF)
|
||
}
|
||
mutating func writeUInt32LE(_ value: UInt32, at offset: Int) {
|
||
self[offset] = UInt8(value & 0xFF)
|
||
self[offset + 1] = UInt8((value >> 8) & 0xFF)
|
||
self[offset + 2] = UInt8((value >> 16) & 0xFF)
|
||
self[offset + 3] = UInt8((value >> 24) & 0xFF)
|
||
}
|
||
|
||
func readInt16LE(at offset: Int) -> Int16 {
|
||
let lo = UInt16(self[offset])
|
||
let hi = UInt16(self[offset + 1])
|
||
return Int16(bitPattern: lo | (hi << 8))
|
||
}
|
||
func readInt32LE(at offset: Int) -> Int32 {
|
||
let b0 = UInt32(self[offset])
|
||
let b1 = UInt32(self[offset + 1])
|
||
let b2 = UInt32(self[offset + 2])
|
||
let b3 = UInt32(self[offset + 3])
|
||
return Int32(bitPattern: b0 | (b1 << 8) | (b2 << 16) | (b3 << 24))
|
||
}
|
||
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)
|
||
}
|
||
}
|
||
|