feat: prioritise UWB ranging over GPS for distance display

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 <noreply@anthropic.com>
This commit is contained in:
sl-ios 2026-04-06 17:24:39 -04:00
parent 313e84a516
commit b2bda0f467
2 changed files with 90 additions and 10 deletions

View File

@ -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
}
}

View File

@ -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