From c472668d7a515953ec4457138102c1b29c757717 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Sat, 4 Apr 2026 12:22:17 -0400 Subject: [PATCH] =?UTF-8?q?feat:=20Merge=20SaltyTag=20BLE=20=E2=80=94=20GP?= =?UTF-8?q?S/IMU=20streaming=20to=20UWB=20tag,=20anchor=20display,=20UWB?= =?UTF-8?q?=20position=20authority?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BLE Protocol (from SaltyTag Flutter app, exact binary format): Service: 12345678-1234-5678-1234-56789abcdef0 GPS char: …def3 IMU char: …def4 Ranging char: …def5 (notify) BLEManager.swift — CoreBluetooth central: - Scans for peripherals advertising the service UUID; name prefix "UWB_TAG" - 15 s scan timeout, auto-reconnect with 2 s backoff on disconnect - Exposes sendGPS(Data) + sendIMU(Data); gpsStreamEnabled / imuStreamEnabled toggles - Subscribes to ranging notifications → parses → publishes anchors[] BLEPackets.swift — exact binary encoders matching SaltyTag firmware expectations: - gpsPacket(CLLocation) → 20 bytes LE: lat×1e7, lon×1e7, alt×10(Int16), speed×100(UInt16), heading×100(UInt16), accuracy×10(UInt8), fix_type, ts_ms_low32 - imuPacket(CMDeviceMotion) → 22 bytes LE: accel XYZ milli-g (already in g from CoreMotion), gyro XYZ centi-deg/s (rad/s × 5729.578), mag XYZ μT, ts_ms_low32 - parseRanging(Data) → [AnchorInfo]: count byte + 9 bytes/anchor (index, Int32-mm range, Int16×10 RSSI, UInt16 age_ms) AnchorInfo.swift — anchor model with 3 s staleness check BLEStatusView.swift — "BLE Tag" tab (3rd tab in ContentView): - Connection card: state dot, peripheral name, Scan/Stop/Disconnect button - GPS→Tag and IMU→Tag streaming toggles (5 Hz / 10 Hz rates shown) - Anchor list matching SaltyTag UI: freshness dot, A{id} label, range, RSSI, STALE badge Green label if <5 m, orange if ≥5 m, gray if stale SensorManager: - Owns BLEManager; observes connectionState via Combine → starts/stops BLE timers - BLE GPS timer: 200 ms (5 Hz), sends lastKnownLocation via BLEPackets.gpsPacket - BLE IMU timer: 100 ms (10 Hz), sends lastKnownMotion via BLEPackets.imuPacket - lastKnownMotion updated from 100 Hz CMDeviceMotion callback - ensureSensorsRunning() called on BLE connect (sensors start even without Follow-Me) - Subscribes to saltybot/uwb/tag/position — Orin-fused phone absolute position (robot RTK GPS + UWB tag offset = cm-accurate phone position) Phone position source hierarchy (updateBestPhonePosition): 1. saltybot/uwb/tag/position fresh < 3 s → UWB authority (more accurate than phone GPS) 2. CoreLocation GPS fallback - phonePositionSource: PhonePositionSource (.uwb(accuracyM) | .gps(accuracyM)) - userLocation always set to best source; MQTT publish to saltybot/ios/gps unchanged MapContentView: - positionSourceBadge (top-left, below UWB badge): "Position: UWB 2cm" or "Position: GPS 5m" with waveform icon (UWB) or location icon (GPS) Info.plist: - NSBluetoothAlwaysUsageDescription added - NSMotionUsageDescription updated (SAUL-T-MOTE branding) - UIBackgroundModes: added bluetooth-central Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee.xcodeproj/project.pbxproj | 16 ++ SulTee/SulTee/AnchorInfo.swift | 26 +++ SulTee/SulTee/BLEManager.swift | 213 +++++++++++++++++++++ SulTee/SulTee/BLEPackets.swift | 188 +++++++++++++++++++ SulTee/SulTee/BLEStatusView.swift | 190 +++++++++++++++++++ SulTee/SulTee/ContentView.swift | 2 + SulTee/SulTee/Info.plist | 5 +- SulTee/SulTee/MapContentView.swift | 24 ++- SulTee/SulTee/SensorManager.swift | 238 +++++++++++++++++------- 9 files changed, 833 insertions(+), 69 deletions(-) create mode 100644 SulTee/SulTee/AnchorInfo.swift create mode 100644 SulTee/SulTee/BLEManager.swift create mode 100644 SulTee/SulTee/BLEPackets.swift create mode 100644 SulTee/SulTee/BLEStatusView.swift diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index a65c916..969f5f3 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -15,6 +15,10 @@ A100000100000000000009AA /* MQTTClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000009AB /* MQTTClient.swift */; }; A10000010000000000000AAA /* MapContentView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000AAB /* MapContentView.swift */; }; A10000010000000000000BAA /* UWBModels.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000BAB /* UWBModels.swift */; }; + A10000010000000000000CAA /* BLEManager.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000CAB /* BLEManager.swift */; }; + A10000010000000000000DAA /* BLEPackets.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000DAB /* BLEPackets.swift */; }; + A10000010000000000000EAA /* AnchorInfo.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000EAB /* AnchorInfo.swift */; }; + A10000010000000000000FAA /* BLEStatusView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A10000010000000000000FAB /* BLEStatusView.swift */; }; /* End PBXBuildFile section */ /* Begin PBXFileReference section */ @@ -28,6 +32,10 @@ A100000100000000000009AB /* MQTTClient.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MQTTClient.swift; sourceTree = ""; }; A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = ""; }; A10000010000000000000BAB /* UWBModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = UWBModels.swift; sourceTree = ""; }; + A10000010000000000000CAB /* BLEManager.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = BLEManager.swift; sourceTree = ""; }; + A10000010000000000000DAB /* BLEPackets.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = BLEPackets.swift; sourceTree = ""; }; + A10000010000000000000EAB /* AnchorInfo.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = AnchorInfo.swift; sourceTree = ""; }; + A10000010000000000000FAB /* BLEStatusView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = BLEStatusView.swift; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ @@ -59,6 +67,10 @@ A100000100000000000009AB /* MQTTClient.swift */, A10000010000000000000AAB /* MapContentView.swift */, A10000010000000000000BAB /* UWBModels.swift */, + A10000010000000000000CAB /* BLEManager.swift */, + A10000010000000000000DAB /* BLEPackets.swift */, + A10000010000000000000EAB /* AnchorInfo.swift */, + A10000010000000000000FAB /* BLEStatusView.swift */, A100000100000000000005AB /* Assets.xcassets */, A100000100000000000006AB /* Info.plist */, ); @@ -150,6 +162,10 @@ A100000100000000000009AA /* MQTTClient.swift in Sources */, A10000010000000000000AAA /* MapContentView.swift in Sources */, A10000010000000000000BAA /* UWBModels.swift in Sources */, + A10000010000000000000CAA /* BLEManager.swift in Sources */, + A10000010000000000000DAA /* BLEPackets.swift in Sources */, + A10000010000000000000EAA /* AnchorInfo.swift in Sources */, + A10000010000000000000FAA /* BLEStatusView.swift in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/SulTee/SulTee/AnchorInfo.swift b/SulTee/SulTee/AnchorInfo.swift new file mode 100644 index 0000000..fef8c36 --- /dev/null +++ b/SulTee/SulTee/AnchorInfo.swift @@ -0,0 +1,26 @@ +import Foundation + +/// A single UWB anchor measurement received via BLE notification. +struct AnchorInfo: Identifiable { + let id: UInt8 + let rangeMetres: Double + let rssiDBm: Double + let ageMs: UInt16 // age reported by tag firmware + let receivedAt: Date + + /// True when the measurement is more than 3 seconds old (local wall-clock). + var isStale: Bool { Date().timeIntervalSince(receivedAt) > 3.0 } + + /// Display string for the anchor identifier. + var label: String { "A\(id)" } + + /// Formatted range string matching the Flutter app style. + var rangeString: String { + rangeMetres < 10 + ? String(format: "%.2f m", rangeMetres) + : String(format: "%.1f m", rangeMetres) + } + + /// Formatted RSSI string. + var rssiString: String { "\(Int(rssiDBm.rounded())) dBm" } +} diff --git a/SulTee/SulTee/BLEManager.swift b/SulTee/SulTee/BLEManager.swift new file mode 100644 index 0000000..376cd7c --- /dev/null +++ b/SulTee/SulTee/BLEManager.swift @@ -0,0 +1,213 @@ +import Foundation +import CoreBluetooth + +/// CoreBluetooth manager that scans for, connects to, and communicates with +/// the SaltyBot UWB tag (firmware device name prefix: "UWB_TAG"). +/// +/// - Sends GPS packets to the GPS characteristic (5 Hz, driven by SensorManager) +/// - Sends IMU packets to the IMU characteristic (10 Hz, driven by SensorManager) +/// - Receives ranging notifications and exposes them as `anchors` +/// - Auto-reconnects after disconnect (re-scans after 2 s) +final class BLEManager: NSObject, ObservableObject { + + // MARK: - Service / characteristic UUIDs (from SaltyTag firmware) + + static let serviceUUID = CBUUID(string: "12345678-1234-5678-1234-56789abcdef0") + static let gpsCharUUID = CBUUID(string: "12345678-1234-5678-1234-56789abcdef3") + static let imuCharUUID = CBUUID(string: "12345678-1234-5678-1234-56789abcdef4") + static let rangeCharUUID = CBUUID(string: "12345678-1234-5678-1234-56789abcdef5") + + // MARK: - Published state + + enum ConnectionState: String { + case idle, scanning, connecting, connected, disconnected + } + + @Published var connectionState: ConnectionState = .idle + @Published var peripheralName: String? = nil + @Published var anchors: [AnchorInfo] = [] + @Published var gpsStreamEnabled: Bool = true + @Published var imuStreamEnabled: Bool = true + + var isConnected: Bool { connectionState == .connected } + + // MARK: - Private + + private var central: CBCentralManager! + private var peripheral: CBPeripheral? + private var gpsChar: CBCharacteristic? + private var imuChar: CBCharacteristic? + private var scanTimer: Timer? + private var autoReconnect = false + + override init() { + super.init() + central = CBCentralManager(delegate: self, + queue: DispatchQueue(label: "ble.queue", qos: .utility)) + } + + // MARK: - Public API + + /// Begin scanning for a UWB_TAG peripheral. + func startScan() { + autoReconnect = true + guard central.state == .poweredOn else { return } + doStartScan() + } + + /// Stop scanning and cancel any active connection. + func disconnect() { + autoReconnect = false + stopScan() + if let p = peripheral { central.cancelPeripheralConnection(p) } + peripheral = nil; gpsChar = nil; imuChar = nil + DispatchQueue.main.async { self.connectionState = .idle; self.peripheralName = nil } + } + + /// Write a pre-built GPS packet to the tag. Call at 5 Hz. + func sendGPS(_ data: Data) { + guard gpsStreamEnabled, isConnected, + let p = peripheral, let c = gpsChar else { return } + p.writeValue(data, for: c, type: .withoutResponse) + } + + /// Write a pre-built IMU packet to the tag. Call at 10 Hz. + func sendIMU(_ data: Data) { + guard imuStreamEnabled, isConnected, + let p = peripheral, let c = imuChar else { return } + p.writeValue(data, for: c, type: .withoutResponse) + } + + // MARK: - Internal helpers + + private func doStartScan() { + DispatchQueue.main.async { self.connectionState = .scanning } + central.scanForPeripherals(withServices: [Self.serviceUUID], + options: [CBCentralManagerScanOptionAllowDuplicatesKey: false]) + // Auto-stop after 15 s if nothing found + scanTimer?.invalidate() + scanTimer = Timer.scheduledTimer(withTimeInterval: 15, repeats: false) { [weak self] _ in + guard let self else { return } + self.stopScan() + DispatchQueue.main.async { self.connectionState = .idle } + } + } + + private func stopScan() { + central.stopScan() + scanTimer?.invalidate() + scanTimer = nil + } + + private func reconnectAfterDelay() { + guard autoReconnect else { return } + DispatchQueue.main.asyncAfter(deadline: .now() + 2) { [weak self] in + guard let self, self.autoReconnect, self.central.state == .poweredOn else { return } + self.doStartScan() + } + } +} + +// MARK: - CBCentralManagerDelegate + +extension BLEManager: CBCentralManagerDelegate { + + func centralManagerDidUpdateState(_ central: CBCentralManager) { + if central.state == .poweredOn && autoReconnect { + doStartScan() + } + } + + func centralManager(_ central: CBCentralManager, + didDiscover peripheral: CBPeripheral, + advertisementData: [String: Any], + rssi RSSI: NSNumber) { + // Match by service advertisement or device name prefix + let name = peripheral.name ?? "" + guard name.hasPrefix("UWB_TAG") || name.isEmpty == false else { return } + stopScan() + self.peripheral = peripheral + peripheral.delegate = self + DispatchQueue.main.async { self.connectionState = .connecting } + central.connect(peripheral, options: [ + CBConnectPeripheralOptionNotifyOnDisconnectionKey: true + ]) + } + + func centralManager(_ central: CBCentralManager, + didConnect peripheral: CBPeripheral) { + DispatchQueue.main.async { self.peripheralName = peripheral.name } + peripheral.discoverServices([Self.serviceUUID]) + } + + func centralManager(_ central: CBCentralManager, + didDisconnectPeripheral peripheral: CBPeripheral, + error: Error?) { + self.peripheral = nil; gpsChar = nil; imuChar = nil + DispatchQueue.main.async { + self.connectionState = .disconnected + self.anchors = [] + } + reconnectAfterDelay() + } + + func centralManager(_ central: CBCentralManager, + didFailToConnect peripheral: CBPeripheral, + error: Error?) { + self.peripheral = nil + DispatchQueue.main.async { self.connectionState = .idle } + reconnectAfterDelay() + } +} + +// MARK: - CBPeripheralDelegate + +extension BLEManager: CBPeripheralDelegate { + + func peripheral(_ peripheral: CBPeripheral, + didDiscoverServices error: Error?) { + guard let services = peripheral.services else { return } + for svc in services where svc.uuid == Self.serviceUUID { + peripheral.discoverCharacteristics( + [Self.gpsCharUUID, Self.imuCharUUID, Self.rangeCharUUID], + for: svc + ) + } + } + + func peripheral(_ peripheral: CBPeripheral, + didDiscoverCharacteristicsFor service: CBService, + error: Error?) { + guard let chars = service.characteristics else { return } + for c in chars { + switch c.uuid { + case Self.gpsCharUUID: gpsChar = c + case Self.imuCharUUID: imuChar = c + case Self.rangeCharUUID: + peripheral.setNotifyValue(true, for: c) + default: break + } + } + // All characteristics found → mark connected + if gpsChar != nil && imuChar != nil { + DispatchQueue.main.async { self.connectionState = .connected } + } + } + + func peripheral(_ peripheral: CBPeripheral, + didUpdateValueFor characteristic: CBCharacteristic, + error: Error?) { + guard characteristic.uuid == Self.rangeCharUUID, + let data = characteristic.value else { return } + let parsed = BLEPackets.parseRanging(data) + DispatchQueue.main.async { self.anchors = parsed } + } + + func peripheral(_ peripheral: CBPeripheral, + didUpdateNotificationStateFor characteristic: CBCharacteristic, + error: Error?) { + if let err = error { + print("[BLE] notify subscribe error: \(err)") + } + } +} diff --git a/SulTee/SulTee/BLEPackets.swift b/SulTee/SulTee/BLEPackets.swift new file mode 100644 index 0000000..fb655bb --- /dev/null +++ b/SulTee/SulTee/BLEPackets.swift @@ -0,0 +1,188 @@ +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 + // + // [0] Uint8 anchor count N + // Per anchor (9 bytes, offset = 1 + i×9): + // [+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] { + guard data.count >= 1 else { return [] } + let count = Int(data[0]) + let now = Date() + var result: [AnchorInfo] = [] + + for i in 0..> 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) + } +} + +// MARK: - Safe integer clamping + +private extension Int16 { + init(clamping value: Int64) { + self = Int16(max(Int64(Int16.min), min(Int64(Int16.max), value))) + } +} +private extension Int32 { + init(clamping value: Int64) { + self = Int32(max(Int64(Int32.min), min(Int64(Int32.max), value))) + } +} +private extension UInt16 { + init(clamping value: Int64) { + self = UInt16(max(Int64(0), min(Int64(UInt16.max), value))) + } +} +private extension UInt8 { + init(clamping value: Int64) { + self = UInt8(max(Int64(0), min(Int64(UInt8.max), value))) + } +} diff --git a/SulTee/SulTee/BLEStatusView.swift b/SulTee/SulTee/BLEStatusView.swift new file mode 100644 index 0000000..2f4d559 --- /dev/null +++ b/SulTee/SulTee/BLEStatusView.swift @@ -0,0 +1,190 @@ +import SwiftUI + +/// "BLE Tag" tab — shows connection controls, streaming toggles, and live anchor data. +struct BLEStatusView: View { + @EnvironmentObject var sensor: SensorManager + + private var ble: BLEManager { sensor.ble } + + var body: some View { + NavigationStack { + List { + connectionSection + if ble.isConnected { streamingSection } + anchorsSection + } + .navigationTitle("BLE Tag") + } + } + + // MARK: - Connection section + + private var connectionSection: some View { + Section("UWB Tag Connection") { + HStack(spacing: 12) { + Circle() + .fill(stateColor) + .frame(width: 12, height: 12) + .shadow(color: stateColor.opacity(0.6), radius: ble.isConnected ? 4 : 0) + VStack(alignment: .leading, spacing: 2) { + Text(stateLabel).font(.headline) + if let name = ble.peripheralName { + Text(name).font(.caption).foregroundStyle(.secondary) + } + } + Spacer() + connectionButton + } + .padding(.vertical, 4) + } + } + + private var connectionButton: some View { + Group { + switch ble.connectionState { + case .idle, .disconnected: + Button("Scan") { ble.startScan() } + .buttonStyle(.borderedProminent) + .controlSize(.small) + case .scanning: + HStack(spacing: 6) { + ProgressView().controlSize(.small) + Button("Stop") { ble.disconnect() } + .buttonStyle(.bordered) + .controlSize(.small) + } + case .connecting: + HStack(spacing: 6) { + ProgressView().controlSize(.small) + Text("Connecting…").font(.caption).foregroundStyle(.secondary) + } + case .connected: + Button("Disconnect", role: .destructive) { ble.disconnect() } + .buttonStyle(.bordered) + .controlSize(.small) + } + } + } + + // MARK: - Streaming toggles + + private var streamingSection: some View { + Section("Data Streaming") { + Toggle(isOn: $sensor.ble.gpsStreamEnabled) { + Label { + VStack(alignment: .leading, spacing: 2) { + Text("GPS → Tag") + Text("5 Hz · 20 bytes/packet") + .font(.caption).foregroundStyle(.secondary) + } + } icon: { + Image(systemName: "location.fill") + .foregroundStyle(.blue) + } + } + Toggle(isOn: $sensor.ble.imuStreamEnabled) { + Label { + VStack(alignment: .leading, spacing: 2) { + Text("IMU → Tag") + Text("10 Hz · 22 bytes/packet (accel + gyro + mag)") + .font(.caption).foregroundStyle(.secondary) + } + } icon: { + Image(systemName: "gyroscope") + .foregroundStyle(.purple) + } + } + } + } + + // MARK: - Anchor section + + private var anchorsSection: some View { + Section { + if ble.anchors.isEmpty { + Text(ble.isConnected + ? "Waiting for ranging data…" + : "Connect to a UWB tag to see anchors") + .foregroundStyle(.secondary) + .font(.callout) + } else { + ForEach(ble.anchors.sorted(by: { $0.id < $1.id })) { anchor in + anchorRow(anchor) + } + } + } header: { + HStack { + Text("📡 UWB Anchors") + if !ble.anchors.isEmpty { + Text("(\(ble.anchors.count))") + .foregroundStyle(.secondary) + } + } + } + } + + private func anchorRow(_ anchor: AnchorInfo) -> some View { + HStack(spacing: 12) { + // Freshness dot + Circle() + .fill(anchor.isStale ? Color.gray : Color.green) + .frame(width: 8, height: 8) + + // Anchor ID + Text(anchor.label) + .font(.headline) + .foregroundStyle(anchorLabelColor(anchor)) + .frame(width: 28, alignment: .leading) + + // Range + Text(anchor.rangeString) + .font(.system(size: 18, weight: .bold, design: .monospaced)) + .foregroundStyle(anchor.isStale ? .secondary : .primary) + + Spacer() + + VStack(alignment: .trailing, spacing: 2) { + Text(anchor.rssiString) + .font(.system(size: 11)) + .foregroundStyle(.secondary) + if anchor.isStale { + Text("STALE") + .font(.system(size: 10, weight: .bold)) + .foregroundStyle(.red) + } + } + } + .padding(.vertical, 4) + } + + // MARK: - Helpers + + private var stateColor: Color { + switch ble.connectionState { + case .connected: return .green + case .connecting: return .yellow + case .scanning: return .blue + default: return .gray + } + } + + private var stateLabel: String { + switch ble.connectionState { + case .idle: return "Not Connected" + case .scanning: return "Scanning…" + case .connecting: return "Connecting…" + case .connected: return "Connected" + case .disconnected: return "Disconnected" + } + } + + private func anchorLabelColor(_ anchor: AnchorInfo) -> Color { + guard !anchor.isStale else { return .gray } + return anchor.rangeMetres < 5 ? .green : .orange + } +} + +#Preview { + BLEStatusView() + .environmentObject(SensorManager()) +} diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift index 6ca50da..b3fb875 100644 --- a/SulTee/SulTee/ContentView.swift +++ b/SulTee/SulTee/ContentView.swift @@ -11,6 +11,8 @@ struct ContentView: View { .tabItem { Label("Status", systemImage: "antenna.radiowaves.left.and.right") } MapContentView() .tabItem { Label("Map", systemImage: "map.fill") } + BLEStatusView() + .tabItem { Label("BLE Tag", systemImage: "dot.radiowaves.right") } } } } diff --git a/SulTee/SulTee/Info.plist b/SulTee/SulTee/Info.plist index d00c04b..1052725 100644 --- a/SulTee/SulTee/Info.plist +++ b/SulTee/SulTee/Info.plist @@ -26,12 +26,15 @@ Sul-Tee streams your GPS location continuously to SaltyBot for follow-me mode. Background location is required to keep streaming when the phone is locked. NSLocationWhenInUseUsageDescription Sul-Tee streams your GPS location to SaltyBot for follow-me mode. + NSBluetoothAlwaysUsageDescription + SAUL-T-MOTE connects to the SaltyBot UWB tag via Bluetooth to stream GPS and IMU data and receive anchor ranging measurements. NSMotionUsageDescription - Sul-Tee streams IMU and barometer data to SaltyBot for follow-me stabilization. + SAUL-T-MOTE streams IMU (accelerometer, gyroscope, magnetometer) to the SaltyBot UWB tag and Orin for follow-me stabilization. UIBackgroundModes location external-accessory + bluetooth-central UIDeviceFamily diff --git a/SulTee/SulTee/MapContentView.swift b/SulTee/SulTee/MapContentView.swift index d317324..4f2ce85 100644 --- a/SulTee/SulTee/MapContentView.swift +++ b/SulTee/SulTee/MapContentView.swift @@ -120,7 +120,12 @@ struct MapContentView: View { .mapStyle(.standard(elevation: .realistic)) .onMapCameraChange { _ in followUser = false } .overlay(alignment: .topTrailing) { recenterButton } - .overlay(alignment: .topLeading) { uwbBadge } + .overlay(alignment: .topLeading) { + VStack(alignment: .leading, spacing: 6) { + uwbBadge + positionSourceBadge + } + } } // MARK: - UWB status badge (top-left) @@ -156,6 +161,23 @@ struct MapContentView: View { .padding(.leading, 16) } + // MARK: - Phone position source badge + + private var positionSourceBadge: some View { + HStack(spacing: 6) { + Image(systemName: sensor.phonePositionSource.isUWB + ? "waveform.badge.magnifyingglass" : "location.fill") + .font(.caption2) + .foregroundStyle(sensor.phonePositionSource.isUWB ? .green : .blue) + Text("Position: \(sensor.phonePositionSource.label)") + .font(.caption2.bold()) + } + .padding(.horizontal, 10) + .padding(.vertical, 5) + .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 8)) + .padding(.leading, 16) + } + // MARK: - Re-centre button (top-right) private var recenterButton: some View { diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 34a9121..8ed014e 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -4,8 +4,25 @@ import CoreMotion import MapKit import Combine -/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS -/// and UWB data, and exposes state for the map and status views. +// MARK: - Phone position source + +enum PhonePositionSource { + case uwb(accuracyM: Double) // robot RTK GPS + UWB offset, computed by Orin + case gps(accuracyM: Double) // CoreLocation GPS + case unknown + + var label: String { + switch self { + case .uwb(let a): return String(format: "UWB %.0fcm", a * 100) + case .gps(let a): return String(format: "GPS %.0fm", a) + case .unknown: return "—" + } + } + var isUWB: Bool { if case .uwb = self { return true }; return false } +} + +/// Manages all iPhone sensors, BLE tag streaming, MQTT telemetry, and exposes +/// fused position state for the map and status views. final class SensorManager: NSObject, ObservableObject { // MARK: - Streaming state @@ -20,10 +37,12 @@ final class SensorManager: NSObject, ObservableObject { @Published var headingRate: Double = 0 @Published var baroRate: Double = 0 - // MARK: - User (phone) position + // MARK: - User (phone) position — fused from UWB-tag or CoreLocation - @Published var userLocation: CLLocationCoordinate2D? = nil - @Published var userBreadcrumbs: [CLLocationCoordinate2D] = [] + /// Best available phone position. Updated by `updateBestPhonePosition()`. + @Published var userLocation: CLLocationCoordinate2D? = nil + @Published var userBreadcrumbs: [CLLocationCoordinate2D] = [] + @Published var phonePositionSource: PhonePositionSource = .unknown // MARK: - Robot position (saltybot/phone/gps) @@ -32,10 +51,10 @@ final class SensorManager: NSObject, ObservableObject { @Published var robotSpeed: Double = 0 @Published var distanceToRobot: Double? = nil - // MARK: - UWB (saltybot/uwb/range + saltybot/uwb/position) + // MARK: - UWB local data (saltybot/uwb/range + saltybot/uwb/position) @Published var uwbPosition: UWBPosition? = nil - @Published var uwbRanges: [String: UWBRange] = [:] // anchorID → UWBRange + @Published var uwbRanges: [String: UWBRange] = [:] @Published var uwbActive: Bool = false // MARK: - Follow settings @@ -43,9 +62,13 @@ final class SensorManager: NSObject, ObservableObject { @Published var followMode: FollowMode = .gps @Published var followPreset: FollowPreset = .medium + // MARK: - BLE tag + + let ble = BLEManager() + // MARK: - WebSocket config - static let defaultOrinURL = "ws://100.64.0.2:9090" + static let defaultOrinURL = "ws://100.64.0.2:9090" private static let orinURLKey = "orinURL" private(set) var ws: WebSocketClient @@ -65,25 +88,39 @@ final class SensorManager: NSObject, ObservableObject { clientID: "saul-t-mote-\(UUID().uuidString.prefix(8))" )) - private static let iosGPSTopic = "saltybot/ios/gps" - private static let robotGPSTopic = "saltybot/phone/gps" - private static let uwbRangeTopic = "saltybot/uwb/range" - private static let uwbPositionTopic = "saltybot/uwb/position" - private static let followModeTopic = "saltybot/follow/mode" - private static let followRangeTopic = "saltybot/follow/range" - private static let maxBreadcrumbs = 60 - private static let uwbStaleSeconds = 3.0 + private static let iosGPSTopic = "saltybot/ios/gps" + private static let robotGPSTopic = "saltybot/phone/gps" + private static let uwbRangeTopic = "saltybot/uwb/range" + private static let uwbPositionTopic = "saltybot/uwb/position" + private static let uwbTagPosTopic = "saltybot/uwb/tag/position" // Orin-fused phone position + private static let followModeTopic = "saltybot/follow/mode" + private static let followRangeTopic = "saltybot/follow/range" + private static let maxBreadcrumbs = 60 + private static let uwbStaleSeconds = 3.0 - private var lastKnownLocation: CLLocation? - private var mqttPublishTimer: Timer? + // MARK: - Internal sensor state + + private var lastKnownLocation: CLLocation? + private var lastKnownMotion: CMDeviceMotion? + + /// Orin-fused phone absolute position (RTK GPS + UWB offset). + private var uwbTagPosition: (coord: CLLocationCoordinate2D, accuracyM: Double, ts: Date)? + + // MARK: - Timers + + private var mqttGPSTimer: Timer? // 1 Hz MQTT publish + private var bleGPSTimer: Timer? // 5 Hz BLE GPS write + private var bleIMUTimer: Timer? // 10 Hz BLE IMU write private var uwbStalenessTimer: Timer? + private var rateTimer: Timer? - // MARK: - Sensors + // MARK: - CoreMotion / CoreLocation private let locationManager = CLLocationManager() private let motionManager = CMMotionManager() private let altimeter = CMAltimeter() private var cancellables = Set() + private var bleCancellables = Set() // MARK: - Rate counters @@ -91,7 +128,6 @@ final class SensorManager: NSObject, ObservableObject { private var imuCounts: [Date] = [] private var headingCounts: [Date] = [] private var baroCounts: [Date] = [] - private var rateTimer: Timer? // MARK: - Init @@ -118,12 +154,28 @@ final class SensorManager: NSObject, ObservableObject { case Self.robotGPSTopic: self.handleRobotGPS(payload) case Self.uwbRangeTopic: self.handleUWBRange(payload) case Self.uwbPositionTopic: self.handleUWBPosition(payload) + case Self.uwbTagPosTopic: self.handleUWBTagPosition(payload) default: break } } + + // Start BLE timers whenever the tag connects; stop when it disconnects + ble.$connectionState + .receive(on: DispatchQueue.main) + .sink { [weak self] state in + guard let self else { return } + if state == .connected { + self.startBLETimers() + // Ensure sensors are running (needed even without Follow-Me mode) + self.ensureSensorsRunning() + } else { + self.stopBLETimers() + } + } + .store(in: &bleCancellables) } - // MARK: - Public control + // MARK: - Public control (Follow-Me / WebSocket) func startStreaming() { guard !isStreaming else { return } @@ -133,11 +185,11 @@ final class SensorManager: NSObject, ObservableObject { mqtt.subscribe(topic: Self.robotGPSTopic) mqtt.subscribe(topic: Self.uwbRangeTopic) mqtt.subscribe(topic: Self.uwbPositionTopic) - requestPermissionsAndStartSensors() + mqtt.subscribe(topic: Self.uwbTagPosTopic) + ensureSensorsRunning() startRateTimer() - startMQTTPublishTimer() + startMQTTGPSTimer() startUWBStalenessTimer() - // Publish current follow settings immediately on connect publishFollowMode() publishFollowPreset() } @@ -147,9 +199,10 @@ final class SensorManager: NSObject, ObservableObject { isStreaming = false ws.disconnect() mqtt.disconnect() - stopSensors() - rateTimer?.invalidate(); rateTimer = nil - mqttPublishTimer?.invalidate(); mqttPublishTimer = nil + // Keep sensors running if BLE is connected; otherwise stop + if !ble.isConnected { stopSensors() } + rateTimer?.invalidate(); rateTimer = nil + mqttGPSTimer?.invalidate(); mqttGPSTimer = nil uwbStalenessTimer?.invalidate(); uwbStalenessTimer = nil } @@ -161,32 +214,56 @@ final class SensorManager: NSObject, ObservableObject { } } - /// Change follow mode and publish to MQTT immediately. func setFollowMode(_ mode: FollowMode) { - followMode = mode - publishFollowMode() + followMode = mode; publishFollowMode() } - /// Change follow range preset and publish to MQTT immediately. func setFollowPreset(_ preset: FollowPreset) { - followPreset = preset - publishFollowPreset() + followPreset = preset; publishFollowPreset() } - // MARK: - MQTT publish helpers + // MARK: - Sensor lifecycle - private func publishFollowMode() { - mqtt.publish(topic: Self.followModeTopic, payload: followMode.mqttPayload) + private func ensureSensorsRunning() { + locationManager.requestAlwaysAuthorization() + locationManager.startUpdatingLocation() + locationManager.startUpdatingHeading() + if !motionManager.isDeviceMotionActive { startIMU() } + if !altimeter.isRelativeAltitudeAvailable() == false { startBarometer() } } - private func publishFollowPreset() { - mqtt.publish(topic: Self.followRangeTopic, payload: followPreset.mqttPayload) + private func stopSensors() { + locationManager.stopUpdatingLocation() + locationManager.stopUpdatingHeading() + motionManager.stopDeviceMotionUpdates() + altimeter.stopRelativeAltitudeUpdates() + } + + // MARK: - BLE streaming timers + + private func startBLETimers() { + stopBLETimers() + // GPS → tag at 5 Hz (200 ms) + bleGPSTimer = Timer.scheduledTimer(withTimeInterval: 0.2, repeats: true) { [weak self] _ in + guard let self, let loc = self.lastKnownLocation else { return } + self.ble.sendGPS(BLEPackets.gpsPacket(from: loc)) + } + // IMU → tag at 10 Hz (100 ms) + bleIMUTimer = Timer.scheduledTimer(withTimeInterval: 0.1, repeats: true) { [weak self] _ in + guard let self, let motion = self.lastKnownMotion else { return } + self.ble.sendIMU(BLEPackets.imuPacket(from: motion)) + } + } + + private func stopBLETimers() { + bleGPSTimer?.invalidate(); bleGPSTimer = nil + bleIMUTimer?.invalidate(); bleIMUTimer = nil } // MARK: - MQTT GPS publish (1 Hz) - private func startMQTTPublishTimer() { - mqttPublishTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in + private func startMQTTGPSTimer() { + mqttGPSTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in self?.publishGPSToMQTT() } } @@ -208,6 +285,11 @@ final class SensorManager: NSObject, ObservableObject { mqtt.publish(topic: Self.iosGPSTopic, payload: json) } + // MARK: - MQTT follow publish helpers + + private func publishFollowMode() { mqtt.publish(topic: Self.followModeTopic, payload: followMode.mqttPayload) } + private func publishFollowPreset() { mqtt.publish(topic: Self.followRangeTopic, payload: followPreset.mqttPayload) } + // MARK: - Incoming MQTT handlers private func handleRobotGPS(_ payload: String) { @@ -227,9 +309,7 @@ final class SensorManager: NSObject, ObservableObject { let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any], let anchorID = json["anchor_id"] as? String, let rangeM = json["range_m"] as? Double else { return } - uwbRanges[anchorID] = UWBRange(anchorID: anchorID, - rangeMetres: rangeM, - timestamp: Date()) + uwbRanges[anchorID] = UWBRange(anchorID: anchorID, rangeMetres: rangeM, timestamp: Date()) } private func handleUWBPosition(_ payload: String) { @@ -242,16 +322,56 @@ final class SensorManager: NSObject, ObservableObject { uwbActive = true } + /// Orin-fused phone absolute position: robot RTK GPS + UWB offset. + /// This is the most accurate phone position when UWB is in range. + private func handleUWBTagPosition(_ payload: String) { + guard let data = payload.data(using: .utf8), + let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any], + let lat = json["lat"] as? Double, + let lon = json["lon"] as? Double else { return } + let accuracy = (json["accuracy_m"] as? Double) ?? 0.02 // default 2 cm for UWB + let coord = CLLocationCoordinate2D(latitude: lat, longitude: lon) + uwbTagPosition = (coord: coord, accuracyM: accuracy, ts: Date()) + updateBestPhonePosition() + } + + // MARK: - Phone position source selection + + /// Selects the best available phone position: + /// 1. UWB-derived (saltybot/uwb/tag/position) if fresh < 3 s + /// 2. CoreLocation GPS fallback + private func updateBestPhonePosition() { + if let uwb = uwbTagPosition, + Date().timeIntervalSince(uwb.ts) < Self.uwbStaleSeconds { + // Robot RTK + UWB offset is the authority + let coord = uwb.coord + if userLocation != coord { + userLocation = coord + appendBreadcrumb(coord, to: &userBreadcrumbs) + updateDistance() + } + phonePositionSource = .uwb(accuracyM: uwb.accuracyM) + } else if let loc = lastKnownLocation { + let coord = loc.coordinate + if userLocation != coord { + userLocation = coord + appendBreadcrumb(coord, to: &userBreadcrumbs) + updateDistance() + } + phonePositionSource = .gps(accuracyM: max(0, loc.horizontalAccuracy)) + } + } + // MARK: - UWB staleness watchdog private func startUWBStalenessTimer() { uwbStalenessTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in guard let self else { return } let cutoff = Date().addingTimeInterval(-Self.uwbStaleSeconds) - if let pos = self.uwbPosition, pos.timestamp < cutoff { - self.uwbActive = false - } + if let pos = self.uwbPosition, pos.timestamp < cutoff { self.uwbActive = false } self.uwbRanges = self.uwbRanges.filter { $0.value.timestamp > cutoff } + // Re-evaluate phone position source when UWB tag position may have gone stale + self.updateBestPhonePosition() } } @@ -270,28 +390,14 @@ final class SensorManager: NSObject, ObservableObject { distanceToRobot = a.distance(from: b) } - // MARK: - Sensor start / stop - - private func requestPermissionsAndStartSensors() { - locationManager.requestAlwaysAuthorization() - locationManager.startUpdatingLocation() - locationManager.startUpdatingHeading() - startIMU() - startBarometer() - } - - private func stopSensors() { - locationManager.stopUpdatingLocation() - locationManager.stopUpdatingHeading() - motionManager.stopDeviceMotionUpdates() - altimeter.stopRelativeAltitudeUpdates() - } + // MARK: - IMU / Barometer private func startIMU() { guard motionManager.isDeviceMotionAvailable else { return } motionManager.deviceMotionUpdateInterval = 1.0 / 100.0 motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, _ in guard let self, let motion else { return } + self.lastKnownMotion = motion self.recordEvent(in: &self.imuCounts) self.ws.send([ "type": "imu", @@ -358,11 +464,9 @@ extension SensorManager: CLLocationManagerDelegate { func locationManager(_ manager: CLLocationManager, didUpdateLocations locations: [CLLocation]) { guard let loc = locations.last else { return } lastKnownLocation = loc - let coord = loc.coordinate - userLocation = coord - appendBreadcrumb(coord, to: &userBreadcrumbs) - updateDistance() recordEvent(in: &gpsCounts) + // Let the source-selection logic decide whether to use this or UWB-derived position + updateBestPhonePosition() ws.send([ "type": "gps", @@ -402,7 +506,7 @@ extension SensorManager: CLLocationManagerDelegate { func locationManagerDidChangeAuthorization(_ manager: CLLocationManager) { switch manager.authorizationStatus { case .authorizedAlways, .authorizedWhenInUse: - if isStreaming { + if isStreaming || ble.isConnected { manager.startUpdatingLocation() manager.startUpdatingHeading() }