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