From b2bda0f467f81af5c71caf8f0cecba3d075624e3 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 17:24:39 -0400 Subject: [PATCH] feat: prioritise UWB ranging over GPS for distance display MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Distance source priority: 1. BLE Range notify from wearable TAG (cm-accurate, live) 2. MQTT saltybot/uwb/range from Orin anchors 3. GPS coordinate diff — fallback only Changes: - SensorManager: add DistanceSource enum (blueUWB/mqttUWB/gps); replace updateDistance() with updateDistanceToRobot() that checks fresh BLE anchors first, then MQTT UWB ranges, then GPS; subscribe to ble.$anchors so every Range notify triggers re-evaluation; also trigger on MQTT UWB range arrival and BLE disconnect - ContentView: distanceRow now shows source label and icon: green + "UWB (BLE tag)" when BLE anchors are fresh mint + "UWB (Orin anchors)" when MQTT UWB ranges are fresh orange + "GPS estimate" fallback; prefixed with ~ to signal UWB shows cm precision (e.g. "3.84 m"), GPS shows integer metres Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/ContentView.swift | 51 +++++++++++++++++++++++++++---- SulTee/SulTee/SensorManager.swift | 49 ++++++++++++++++++++++++++--- 2 files changed, 90 insertions(+), 10 deletions(-) diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift index b3fb875..5443342 100644 --- a/SulTee/SulTee/ContentView.swift +++ b/SulTee/SulTee/ContentView.swift @@ -125,12 +125,51 @@ private struct StatusView: View { // MARK: Distance row private func distanceRow(_ dist: Double) -> some View { - HStack { - Image(systemName: "arrow.left.and.right") - Text(dist < 1000 - ? "Robot \(Int(dist)) m away" - : String(format: "Robot %.1f km away", dist / 1000)) - .font(.title2).bold() + HStack(spacing: 10) { + Image(systemName: distanceIcon) + .foregroundStyle(distanceColor) + VStack(alignment: .leading, spacing: 2) { + Text(distanceLabel(dist)) + .font(.title2).bold() + Text(distanceSourceLabel) + .font(.caption).foregroundStyle(distanceColor) + } + } + } + + private func distanceLabel(_ dist: Double) -> String { + switch sensor.distanceSource { + case .blueUWB, .mqttUWB: + return dist < 10 + ? String(format: "%.2f m", dist) + : String(format: "%.1f m", dist) + case .gps: + return dist < 1000 + ? "~\(Int(dist)) m" + : String(format: "~%.1f km", dist / 1000) + } + } + + private var distanceSourceLabel: String { + switch sensor.distanceSource { + case .blueUWB: return "UWB (BLE tag)" + case .mqttUWB: return "UWB (Orin anchors)" + case .gps: return "GPS estimate" + } + } + + private var distanceIcon: String { + switch sensor.distanceSource { + case .blueUWB, .mqttUWB: return "dot.radiowaves.left.and.right" + case .gps: return "location.fill" + } + } + + private var distanceColor: Color { + switch sensor.distanceSource { + case .blueUWB: return .green + case .mqttUWB: return .mint + case .gps: return .orange } } diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index af9f6ea..c3b3820 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -51,6 +51,13 @@ final class SensorManager: NSObject, ObservableObject { @Published var robotSpeed: Double = 0 @Published var distanceToRobot: Double? = nil + enum DistanceSource { + case blueUWB // BLE Range notify from wearable TAG (highest accuracy) + case mqttUWB // saltybot/uwb/range from Orin anchors + case gps // CoreLocation coordinate diff (fallback) + } + @Published var distanceSource: DistanceSource = .gps + // MARK: - UWB local data (saltybot/uwb/range + saltybot/uwb/position) @Published var uwbPosition: UWBPosition? = nil @@ -175,9 +182,17 @@ final class SensorManager: NSObject, ObservableObject { self.ensureSensorsRunning() } else { self.stopBLETimers() + // Re-evaluate distance source now that BLE anchors are gone + self.updateDistanceToRobot() } } .store(in: &bleCancellables) + + // Re-evaluate distance on every Range notify from the wearable TAG + ble.$anchors + .receive(on: DispatchQueue.main) + .sink { [weak self] _ in self?.updateDistanceToRobot() } + .store(in: &bleCancellables) } // MARK: - Public control (Follow-Me / WebSocket) @@ -306,7 +321,7 @@ final class SensorManager: NSObject, ObservableObject { robotLocation = coord robotSpeed = (json["speed_ms"] as? Double) ?? 0 appendBreadcrumb(coord, to: &robotBreadcrumbs) - updateDistance() + updateDistanceToRobot() } private func handleUWBRange(_ payload: String) { @@ -315,6 +330,7 @@ final class SensorManager: NSObject, ObservableObject { 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()) + updateDistanceToRobot() } private func handleUWBPosition(_ payload: String) { @@ -353,7 +369,7 @@ final class SensorManager: NSObject, ObservableObject { if userLocation != coord { userLocation = coord appendBreadcrumb(coord, to: &userBreadcrumbs) - updateDistance() + updateDistanceToRobot() } phonePositionSource = .uwb(accuracyM: uwb.accuracyM) } else if let loc = lastKnownLocation { @@ -361,7 +377,7 @@ final class SensorManager: NSObject, ObservableObject { if userLocation != coord { userLocation = coord appendBreadcrumb(coord, to: &userBreadcrumbs) - updateDistance() + updateDistanceToRobot() } phonePositionSource = .gps(accuracyM: max(0, loc.horizontalAccuracy)) } @@ -388,11 +404,36 @@ final class SensorManager: NSObject, ObservableObject { if list.count > Self.maxBreadcrumbs { list.removeFirst() } } - private func updateDistance() { + /// Update distanceToRobot using the highest-accuracy source available: + /// 1. BLE Range notify (wearable TAG) — cm-accurate, freshness checked via AnchorInfo.isStale + /// 2. MQTT UWB ranges (saltybot/uwb/range from Orin anchors) + /// 3. GPS coordinate diff — fallback only + func updateDistanceToRobot() { + // Priority 1: fresh BLE anchor data from the wearable TAG + let freshAnchors = ble.anchors.filter { !$0.isStale } + if !freshAnchors.isEmpty { + let closest = freshAnchors.min(by: { $0.rangeMetres < $1.rangeMetres })! + distanceToRobot = closest.rangeMetres + distanceSource = .blueUWB + return + } + + // Priority 2: MQTT UWB ranges from Orin anchors + let freshRanges = uwbRanges.values.filter { + Date().timeIntervalSince($0.timestamp) < Self.uwbStaleSeconds + } + if let closest = freshRanges.min(by: { $0.rangeMetres < $1.rangeMetres }) { + distanceToRobot = closest.rangeMetres + distanceSource = .mqttUWB + return + } + + // Priority 3: GPS coordinate diff guard let user = userLocation, let robot = robotLocation else { return } let a = CLLocation(latitude: user.latitude, longitude: user.longitude) let b = CLLocation(latitude: robot.latitude, longitude: robot.longitude) distanceToRobot = a.distance(from: b) + distanceSource = .gps } // MARK: - IMU / Barometer