feat: Add UWB integration — follow mode, range presets, UWB status
UWBModels.swift (new):
- FollowMode enum: .gps / .uwb — publishes {"mode":"gps|uwb"} to saltybot/follow/mode
- FollowPreset enum: .close(1.5m) / .medium(3m) / .far(5m) — publishes
{"range_m":N,"preset":"..."} to saltybot/follow/range
- UWBPosition struct: x/y/z + timestamp
- UWBRange struct: anchorID + rangeMetres + timestamp
SensorManager:
- Subscribes to saltybot/uwb/range + saltybot/uwb/position on startStreaming
- handleUWBRange: updates uwbRanges[anchorID] (keyed dict)
- handleUWBPosition: updates uwbPosition + sets uwbActive=true
- UWB staleness watchdog (1Hz timer): clears uwbActive and prunes stale ranges >3s
- setFollowMode(_:) / setFollowPreset(_:): update state + publish to MQTT immediately
- Publishes current follow mode+range on connect
MapContentView:
- UWB status badge (top-left): green/gray dot, "UWB Active|Out of Range",
per-anchor range readouts (e.g. A1 2.34m)
- Follow mode segmented control: GPS | UWB
- Follow range segmented control: Close | Medium | Far (shows metres)
- MapCircle follow-range ring around robot: green inside, orange outside range
- Stats bar: distance turns green + checkmark when user is inside follow range;
UWB (x,y) coord shown when UWB mode active and position known
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
72e3138fb3
commit
4eb2fce08f
@ -14,6 +14,7 @@
|
||||
A100000100000000000005AA /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = A100000100000000000005AB /* Assets.xcassets */; };
|
||||
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 */; };
|
||||
/* End PBXBuildFile section */
|
||||
|
||||
/* Begin PBXFileReference section */
|
||||
@ -26,6 +27,7 @@
|
||||
A100000100000000000007AB /* SulTee.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = SulTee.app; sourceTree = BUILT_PRODUCTS_DIR; };
|
||||
A100000100000000000009AB /* MQTTClient.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MQTTClient.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000BAB /* UWBModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = UWBModels.swift; sourceTree = "<group>"; };
|
||||
/* End PBXFileReference section */
|
||||
|
||||
/* Begin PBXFrameworksBuildPhase section */
|
||||
@ -56,6 +58,7 @@
|
||||
A100000100000000000004AB /* WebSocketClient.swift */,
|
||||
A100000100000000000009AB /* MQTTClient.swift */,
|
||||
A10000010000000000000AAB /* MapContentView.swift */,
|
||||
A10000010000000000000BAB /* UWBModels.swift */,
|
||||
A100000100000000000005AB /* Assets.xcassets */,
|
||||
A100000100000000000006AB /* Info.plist */,
|
||||
);
|
||||
@ -146,6 +149,7 @@
|
||||
A100000100000000000004AA /* WebSocketClient.swift in Sources */,
|
||||
A100000100000000000009AA /* MQTTClient.swift in Sources */,
|
||||
A10000010000000000000AAA /* MapContentView.swift in Sources */,
|
||||
A10000010000000000000BAA /* UWBModels.swift in Sources */,
|
||||
);
|
||||
runOnlyForDeploymentPostprocessing = 0;
|
||||
};
|
||||
|
||||
@ -1,18 +1,33 @@
|
||||
import SwiftUI
|
||||
import MapKit
|
||||
|
||||
extension CLLocationCoordinate2D: @retroactive Equatable {
|
||||
public static func == (lhs: CLLocationCoordinate2D, rhs: CLLocationCoordinate2D) -> Bool {
|
||||
lhs.latitude == rhs.latitude && lhs.longitude == rhs.longitude
|
||||
}
|
||||
}
|
||||
|
||||
/// Full-screen map showing user (blue) and robot (orange) positions,
|
||||
/// a follow-path line between them, and fading breadcrumb trails for both.
|
||||
/// a follow-path line, fading breadcrumb trails, UWB status, and follow controls.
|
||||
struct MapContentView: View {
|
||||
@EnvironmentObject var sensor: SensorManager
|
||||
|
||||
@State private var position: MapCameraPosition = .automatic
|
||||
@State private var followUser = true
|
||||
|
||||
private var insideFollowRange: Bool {
|
||||
guard let d = sensor.distanceToRobot else { return false }
|
||||
return d <= sensor.followPreset.metres
|
||||
}
|
||||
|
||||
var body: some View {
|
||||
ZStack(alignment: .bottom) {
|
||||
map
|
||||
overlay
|
||||
VStack(spacing: 0) {
|
||||
Spacer()
|
||||
followControls
|
||||
statsBar
|
||||
}
|
||||
}
|
||||
.ignoresSafeArea(edges: .top)
|
||||
.onChange(of: sensor.userLocation) { _, coord in
|
||||
@ -20,20 +35,32 @@ struct MapContentView: View {
|
||||
withAnimation(.easeInOut(duration: 0.4)) {
|
||||
position = .camera(MapCamera(
|
||||
centerCoordinate: coord,
|
||||
distance: 400,
|
||||
heading: 0,
|
||||
pitch: 0
|
||||
distance: 400, heading: 0, pitch: 0
|
||||
))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Map
|
||||
// MARK: - Map content
|
||||
|
||||
private var map: some View {
|
||||
Map(position: $position) {
|
||||
|
||||
// ── Follow range circle around robot
|
||||
if let robotLoc = sensor.robotLocation {
|
||||
MapCircle(center: robotLoc, radius: sensor.followPreset.metres)
|
||||
.foregroundStyle(
|
||||
insideFollowRange
|
||||
? Color.green.opacity(0.12)
|
||||
: Color.orange.opacity(0.12)
|
||||
)
|
||||
.stroke(
|
||||
insideFollowRange ? Color.green : Color.orange,
|
||||
style: StrokeStyle(lineWidth: 1.5)
|
||||
)
|
||||
}
|
||||
|
||||
// ── User breadcrumb trail (fading blue dots)
|
||||
let userCrumbs = sensor.userBreadcrumbs
|
||||
ForEach(userCrumbs.indices, id: \.self) { idx in
|
||||
@ -69,15 +96,9 @@ struct MapContentView: View {
|
||||
if let userLoc = sensor.userLocation {
|
||||
Annotation("You", coordinate: userLoc) {
|
||||
ZStack {
|
||||
Circle()
|
||||
.fill(.blue.opacity(0.25))
|
||||
.frame(width: 36, height: 36)
|
||||
Circle()
|
||||
.fill(.blue)
|
||||
.frame(width: 16, height: 16)
|
||||
Circle()
|
||||
.stroke(.white, lineWidth: 2.5)
|
||||
.frame(width: 16, height: 16)
|
||||
Circle().fill(.blue.opacity(0.25)).frame(width: 36, height: 36)
|
||||
Circle().fill(.blue).frame(width: 16, height: 16)
|
||||
Circle().stroke(.white, lineWidth: 2.5).frame(width: 16, height: 16)
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -86,9 +107,7 @@ struct MapContentView: View {
|
||||
if let robotLoc = sensor.robotLocation {
|
||||
Annotation("Robot", coordinate: robotLoc) {
|
||||
ZStack {
|
||||
Circle()
|
||||
.fill(.orange.opacity(0.25))
|
||||
.frame(width: 36, height: 36)
|
||||
Circle().fill(.orange.opacity(0.25)).frame(width: 36, height: 36)
|
||||
Image(systemName: "car.fill")
|
||||
.font(.system(size: 16, weight: .bold))
|
||||
.foregroundStyle(.white)
|
||||
@ -99,27 +118,54 @@ struct MapContentView: View {
|
||||
}
|
||||
}
|
||||
.mapStyle(.standard(elevation: .realistic))
|
||||
.onMapCameraChange { _ in
|
||||
// User dragged map → stop auto-follow
|
||||
followUser = false
|
||||
}
|
||||
.overlay(alignment: .topTrailing) {
|
||||
followButton
|
||||
}
|
||||
.onMapCameraChange { _ in followUser = false }
|
||||
.overlay(alignment: .topTrailing) { recenterButton }
|
||||
.overlay(alignment: .topLeading) { uwbBadge }
|
||||
}
|
||||
|
||||
// MARK: - Re-centre button
|
||||
// MARK: - UWB status badge (top-left)
|
||||
|
||||
private var followButton: some View {
|
||||
private var uwbBadge: some View {
|
||||
VStack(alignment: .leading, spacing: 4) {
|
||||
HStack(spacing: 6) {
|
||||
Circle()
|
||||
.fill(sensor.uwbActive ? Color.green : Color.gray)
|
||||
.frame(width: 9, height: 9)
|
||||
.shadow(color: sensor.uwbActive ? .green : .clear, radius: 4)
|
||||
Text(sensor.uwbActive ? "UWB Active" : "UWB Out of Range")
|
||||
.font(.caption2.bold())
|
||||
.foregroundStyle(sensor.uwbActive ? .primary : .secondary)
|
||||
}
|
||||
|
||||
// Per-anchor ranges
|
||||
let sortedRanges = sensor.uwbRanges.values.sorted { $0.anchorID < $1.anchorID }
|
||||
if !sortedRanges.isEmpty {
|
||||
HStack(spacing: 8) {
|
||||
ForEach(sortedRanges) { r in
|
||||
Text("\(r.anchorID) \(r.rangeMetres, specifier: "%.2f")m")
|
||||
.font(.caption2)
|
||||
.foregroundStyle(.secondary)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
.padding(.horizontal, 10)
|
||||
.padding(.vertical, 7)
|
||||
.background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 10))
|
||||
.padding(.top, 56) // below status bar
|
||||
.padding(.leading, 16)
|
||||
}
|
||||
|
||||
// MARK: - Re-centre button (top-right)
|
||||
|
||||
private var recenterButton: some View {
|
||||
Button {
|
||||
followUser = true
|
||||
if let coord = sensor.userLocation {
|
||||
withAnimation {
|
||||
position = .camera(MapCamera(
|
||||
centerCoordinate: coord,
|
||||
distance: 400,
|
||||
heading: 0,
|
||||
pitch: 0
|
||||
distance: 400, heading: 0, pitch: 0
|
||||
))
|
||||
}
|
||||
}
|
||||
@ -128,24 +174,84 @@ struct MapContentView: View {
|
||||
.padding(10)
|
||||
.background(.ultraThinMaterial, in: Circle())
|
||||
}
|
||||
.padding([.top, .trailing], 16)
|
||||
.padding(.top, 44) // below nav bar
|
||||
.padding(.top, 56)
|
||||
.padding(.trailing, 16)
|
||||
}
|
||||
|
||||
// MARK: - Stats overlay
|
||||
// MARK: - Follow controls panel
|
||||
|
||||
private var overlay: some View {
|
||||
private var followControls: some View {
|
||||
VStack(spacing: 10) {
|
||||
|
||||
// Follow mode: GPS | UWB
|
||||
VStack(alignment: .leading, spacing: 4) {
|
||||
Label("Follow Mode", systemImage: "location.circle")
|
||||
.font(.caption.bold())
|
||||
.foregroundStyle(.secondary)
|
||||
Picker("Follow Mode", selection: Binding(
|
||||
get: { sensor.followMode },
|
||||
set: { sensor.setFollowMode($0) }
|
||||
)) {
|
||||
ForEach(FollowMode.allCases) { mode in
|
||||
Text(mode.rawValue).tag(mode)
|
||||
}
|
||||
}
|
||||
.pickerStyle(.segmented)
|
||||
}
|
||||
|
||||
// Follow range: Close | Medium | Far
|
||||
VStack(alignment: .leading, spacing: 4) {
|
||||
HStack {
|
||||
Label("Follow Range", systemImage: "circle.dashed")
|
||||
.font(.caption.bold())
|
||||
.foregroundStyle(.secondary)
|
||||
Spacer()
|
||||
Text("\(sensor.followPreset.metres, specifier: "%.1f") m")
|
||||
.font(.caption.monospacedDigit())
|
||||
.foregroundStyle(.secondary)
|
||||
}
|
||||
Picker("Range", selection: Binding(
|
||||
get: { sensor.followPreset },
|
||||
set: { sensor.setFollowPreset($0) }
|
||||
)) {
|
||||
ForEach(FollowPreset.allCases) { preset in
|
||||
Text(preset.rawValue).tag(preset)
|
||||
}
|
||||
}
|
||||
.pickerStyle(.segmented)
|
||||
}
|
||||
}
|
||||
.padding(.horizontal, 16)
|
||||
.padding(.vertical, 12)
|
||||
.background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 16))
|
||||
.padding(.horizontal, 16)
|
||||
.padding(.bottom, 8)
|
||||
}
|
||||
|
||||
// MARK: - Stats bar
|
||||
|
||||
private var statsBar: some View {
|
||||
HStack(spacing: 20) {
|
||||
if let dist = sensor.distanceToRobot {
|
||||
statCell(value: distanceString(dist),
|
||||
statCell(
|
||||
value: distanceString(dist),
|
||||
label: "distance",
|
||||
icon: "arrow.left.and.right")
|
||||
icon: insideFollowRange ? "checkmark.circle.fill" : "arrow.left.and.right",
|
||||
tint: insideFollowRange ? .green : .primary
|
||||
)
|
||||
}
|
||||
if sensor.robotSpeed > 0.2 {
|
||||
statCell(value: String(format: "%.1f m/s", sensor.robotSpeed),
|
||||
label: "robot spd",
|
||||
icon: "speedometer")
|
||||
}
|
||||
if sensor.followMode == .uwb, let pos = sensor.uwbPosition {
|
||||
statCell(
|
||||
value: String(format: "(%.1f, %.1f)", pos.x, pos.y),
|
||||
label: "UWB pos",
|
||||
icon: "waveform"
|
||||
)
|
||||
}
|
||||
if !sensor.isStreaming {
|
||||
Text("Start Follow-Me to stream")
|
||||
.font(.caption)
|
||||
@ -159,22 +265,25 @@ struct MapContentView: View {
|
||||
.padding(.horizontal, 16)
|
||||
}
|
||||
|
||||
private func statCell(value: String, label: String, icon: String) -> some View {
|
||||
// MARK: - Helpers
|
||||
|
||||
private func statCell(value: String,
|
||||
label: String,
|
||||
icon: String,
|
||||
tint: Color = .primary) -> some View {
|
||||
HStack(spacing: 6) {
|
||||
Image(systemName: icon)
|
||||
.foregroundStyle(.secondary)
|
||||
.foregroundStyle(tint == .primary ? .secondary : tint)
|
||||
.font(.caption)
|
||||
VStack(alignment: .leading, spacing: 0) {
|
||||
Text(value).font(.headline.monospacedDigit())
|
||||
Text(value).font(.headline.monospacedDigit()).foregroundStyle(tint)
|
||||
Text(label).font(.caption2).foregroundStyle(.secondary)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private func distanceString(_ metres: Double) -> String {
|
||||
metres < 1000
|
||||
? "\(Int(metres)) m"
|
||||
: String(format: "%.1f km", metres / 1000)
|
||||
private func distanceString(_ m: Double) -> String {
|
||||
m < 1000 ? "\(Int(m)) m" : String(format: "%.1f km", m / 1000)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -4,8 +4,8 @@ import CoreMotion
|
||||
import MapKit
|
||||
import Combine
|
||||
|
||||
/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS,
|
||||
/// and exposes state for the map and status views.
|
||||
/// 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.
|
||||
final class SensorManager: NSObject, ObservableObject {
|
||||
|
||||
// MARK: - Streaming state
|
||||
@ -25,17 +25,25 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
@Published var userLocation: CLLocationCoordinate2D? = nil
|
||||
@Published var userBreadcrumbs: [CLLocationCoordinate2D] = []
|
||||
|
||||
// MARK: - Robot position (from MQTT saltybot/phone/gps)
|
||||
// MARK: - Robot position (saltybot/phone/gps)
|
||||
|
||||
@Published var robotLocation: CLLocationCoordinate2D? = nil
|
||||
@Published var robotBreadcrumbs: [CLLocationCoordinate2D] = []
|
||||
@Published var robotSpeed: Double = 0
|
||||
|
||||
// MARK: - Derived
|
||||
|
||||
@Published var distanceToRobot: Double? = nil
|
||||
|
||||
// MARK: - WebSocket config (sensor stream to Orin)
|
||||
// MARK: - UWB (saltybot/uwb/range + saltybot/uwb/position)
|
||||
|
||||
@Published var uwbPosition: UWBPosition? = nil
|
||||
@Published var uwbRanges: [String: UWBRange] = [:] // anchorID → UWBRange
|
||||
@Published var uwbActive: Bool = false
|
||||
|
||||
// MARK: - Follow settings
|
||||
|
||||
@Published var followMode: FollowMode = .gps
|
||||
@Published var followPreset: FollowPreset = .medium
|
||||
|
||||
// MARK: - WebSocket config
|
||||
|
||||
static let defaultOrinURL = "ws://100.64.0.2:9090"
|
||||
private static let orinURLKey = "orinURL"
|
||||
@ -56,12 +64,19 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
password: "mqtt_pass",
|
||||
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 var lastKnownLocation: CLLocation?
|
||||
private var mqttPublishTimer: Timer?
|
||||
private var uwbStalenessTimer: Timer?
|
||||
|
||||
// MARK: - Sensors
|
||||
|
||||
@ -81,8 +96,8 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
// MARK: - Init
|
||||
|
||||
override init() {
|
||||
let urlString = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL
|
||||
self.ws = WebSocketClient(url: URL(string: urlString) ?? URL(string: Self.defaultOrinURL)!)
|
||||
let urlStr = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL
|
||||
self.ws = WebSocketClient(url: URL(string: urlStr) ?? URL(string: Self.defaultOrinURL)!)
|
||||
super.init()
|
||||
|
||||
locationManager.delegate = self
|
||||
@ -98,8 +113,13 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
.store(in: &cancellables)
|
||||
|
||||
mqtt.onMessage = { [weak self] topic, payload in
|
||||
guard let self, topic == Self.robotGPSTopic else { return }
|
||||
self.handleRobotGPS(payload)
|
||||
guard let self else { return }
|
||||
switch topic {
|
||||
case Self.robotGPSTopic: self.handleRobotGPS(payload)
|
||||
case Self.uwbRangeTopic: self.handleUWBRange(payload)
|
||||
case Self.uwbPositionTopic: self.handleUWBPosition(payload)
|
||||
default: break
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -111,9 +131,15 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
ws.connect()
|
||||
mqtt.connect()
|
||||
mqtt.subscribe(topic: Self.robotGPSTopic)
|
||||
mqtt.subscribe(topic: Self.uwbRangeTopic)
|
||||
mqtt.subscribe(topic: Self.uwbPositionTopic)
|
||||
requestPermissionsAndStartSensors()
|
||||
startRateTimer()
|
||||
startMQTTPublishTimer()
|
||||
startUWBStalenessTimer()
|
||||
// Publish current follow settings immediately on connect
|
||||
publishFollowMode()
|
||||
publishFollowPreset()
|
||||
}
|
||||
|
||||
func stopStreaming() {
|
||||
@ -124,6 +150,7 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
stopSensors()
|
||||
rateTimer?.invalidate(); rateTimer = nil
|
||||
mqttPublishTimer?.invalidate(); mqttPublishTimer = nil
|
||||
uwbStalenessTimer?.invalidate(); uwbStalenessTimer = nil
|
||||
}
|
||||
|
||||
func updateURL(_ urlString: String) {
|
||||
@ -134,6 +161,28 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
}
|
||||
}
|
||||
|
||||
/// Change follow mode and publish to MQTT immediately.
|
||||
func setFollowMode(_ mode: FollowMode) {
|
||||
followMode = mode
|
||||
publishFollowMode()
|
||||
}
|
||||
|
||||
/// Change follow range preset and publish to MQTT immediately.
|
||||
func setFollowPreset(_ preset: FollowPreset) {
|
||||
followPreset = preset
|
||||
publishFollowPreset()
|
||||
}
|
||||
|
||||
// MARK: - MQTT 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: - MQTT GPS publish (1 Hz)
|
||||
|
||||
private func startMQTTPublishTimer() {
|
||||
@ -159,14 +208,13 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
mqtt.publish(topic: Self.iosGPSTopic, payload: json)
|
||||
}
|
||||
|
||||
// MARK: - Robot GPS subscription handler
|
||||
// MARK: - Incoming MQTT handlers
|
||||
|
||||
private func handleRobotGPS(_ 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 coord = CLLocationCoordinate2D(latitude: lat, longitude: lon)
|
||||
robotLocation = coord
|
||||
robotSpeed = (json["speed_ms"] as? Double) ?? 0
|
||||
@ -174,6 +222,39 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
updateDistance()
|
||||
}
|
||||
|
||||
private func handleUWBRange(_ payload: String) {
|
||||
guard let data = payload.data(using: .utf8),
|
||||
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())
|
||||
}
|
||||
|
||||
private func handleUWBPosition(_ payload: String) {
|
||||
guard let data = payload.data(using: .utf8),
|
||||
let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any],
|
||||
let x = json["x"] as? Double,
|
||||
let y = json["y"] as? Double,
|
||||
let z = json["z"] as? Double else { return }
|
||||
uwbPosition = UWBPosition(x: x, y: y, z: z, timestamp: Date())
|
||||
uwbActive = true
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
self.uwbRanges = self.uwbRanges.filter { $0.value.timestamp > cutoff }
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Breadcrumbs + distance
|
||||
|
||||
private func appendBreadcrumb(_ coord: CLLocationCoordinate2D,
|
||||
@ -208,7 +289,7 @@ final class SensorManager: NSObject, ObservableObject {
|
||||
|
||||
private func startIMU() {
|
||||
guard motionManager.isDeviceMotionAvailable else { return }
|
||||
motionManager.deviceMotionUpdateInterval = 1.0 / 100.0 // 100 Hz
|
||||
motionManager.deviceMotionUpdateInterval = 1.0 / 100.0
|
||||
motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, _ in
|
||||
guard let self, let motion else { return }
|
||||
self.recordEvent(in: &self.imuCounts)
|
||||
|
||||
51
SulTee/SulTee/UWBModels.swift
Normal file
51
SulTee/SulTee/UWBModels.swift
Normal file
@ -0,0 +1,51 @@
|
||||
import Foundation
|
||||
|
||||
// MARK: - Follow mode
|
||||
|
||||
enum FollowMode: String, CaseIterable, Identifiable {
|
||||
case gps = "GPS"
|
||||
case uwb = "UWB"
|
||||
var id: String { rawValue }
|
||||
|
||||
var mqttPayload: String {
|
||||
"{\"mode\":\"\(rawValue.lowercased())\"}"
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Follow range preset
|
||||
|
||||
enum FollowPreset: String, CaseIterable, Identifiable {
|
||||
case close = "Close"
|
||||
case medium = "Medium"
|
||||
case far = "Far"
|
||||
var id: String { rawValue }
|
||||
|
||||
var metres: Double {
|
||||
switch self {
|
||||
case .close: return 1.5
|
||||
case .medium: return 3.0
|
||||
case .far: return 5.0
|
||||
}
|
||||
}
|
||||
|
||||
var mqttPayload: String {
|
||||
let d: [String: Any] = ["range_m": metres, "preset": rawValue.lowercased()]
|
||||
guard let data = try? JSONSerialization.data(withJSONObject: d),
|
||||
let str = String(data: data, encoding: .utf8) else { return "{}" }
|
||||
return str
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - UWB data types
|
||||
|
||||
struct UWBPosition {
|
||||
let x, y, z: Double
|
||||
let timestamp: Date
|
||||
}
|
||||
|
||||
struct UWBRange: Identifiable {
|
||||
let anchorID: String
|
||||
let rangeMetres: Double
|
||||
let timestamp: Date
|
||||
var id: String { anchorID }
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user