From baab4eaeb2f9f47d28901855d1dccb7e944587a0 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Fri, 3 Apr 2026 16:52:26 -0400 Subject: [PATCH 01/19] feat: iOS companion app - sensor streaming over WebSocket (Issue #709) - SulTee SwiftUI app targeting iOS 17+, iPhone 15 Pro - CoreLocation: dual-frequency GPS (L1+L5) continuous updates, background mode enabled - CoreMotion: 100 Hz IMU (accel + gyro + attitude + gravity), magnetometer via device motion - CMAltimeter: barometer relative altitude + pressure streaming - CLLocationManager heading updates for magnetometer heading - URLSessionWebSocketTask client connecting to ws://192.168.86.158:9090 - JSON protocol: {type, timestamp, data} for gps/imu/heading/baro messages - Auto-reconnect on disconnect (2s backoff) - Haptic feedback on incoming "haptic" messages from bot - Background streaming: UIBackgroundModes location + external-accessory in Info.plist - SwiftUI status UI: connection banner, sensor rate counters (Hz), start/stop follow-me button - Dev team Z37N597UWY (vayrette@gmail.com), bundle ID com.saltylab.sultee Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee.xcodeproj/project.pbxproj | 341 ++++++++++++++++++ .../AppIcon.appiconset/Contents.json | 13 + SulTee/SulTee/Assets.xcassets/Contents.json | 6 + SulTee/SulTee/ContentView.swift | 115 ++++++ SulTee/SulTee/Info.plist | 57 +++ SulTee/SulTee/SensorManager.swift | 230 ++++++++++++ SulTee/SulTee/SulTeeApp.swift | 13 + SulTee/SulTee/WebSocketClient.swift | 114 ++++++ 8 files changed, 889 insertions(+) create mode 100644 SulTee/SulTee.xcodeproj/project.pbxproj create mode 100644 SulTee/SulTee/Assets.xcassets/AppIcon.appiconset/Contents.json create mode 100644 SulTee/SulTee/Assets.xcassets/Contents.json create mode 100644 SulTee/SulTee/ContentView.swift create mode 100644 SulTee/SulTee/Info.plist create mode 100644 SulTee/SulTee/SensorManager.swift create mode 100644 SulTee/SulTee/SulTeeApp.swift create mode 100644 SulTee/SulTee/WebSocketClient.swift diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj new file mode 100644 index 0000000..c0193ec --- /dev/null +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -0,0 +1,341 @@ +// !$*UTF8*$! +{ + archiveVersion = 1; + classes = { + }; + objectVersion = 56; + objects = { + +/* Begin PBXBuildFile section */ + A100000100000000000001AA /* SulTeeApp.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000001AB /* SulTeeApp.swift */; }; + A100000100000000000002AA /* ContentView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000002AB /* ContentView.swift */; }; + A100000100000000000003AA /* SensorManager.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000003AB /* SensorManager.swift */; }; + A100000100000000000004AA /* WebSocketClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000004AB /* WebSocketClient.swift */; }; + A100000100000000000005AA /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = A100000100000000000005AB /* Assets.xcassets */; }; +/* End PBXBuildFile section */ + +/* Begin PBXFileReference section */ + A100000100000000000001AB /* SulTeeApp.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = SulTeeApp.swift; sourceTree = ""; }; + A100000100000000000002AB /* ContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = ContentView.swift; sourceTree = ""; }; + A100000100000000000003AB /* SensorManager.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = SensorManager.swift; sourceTree = ""; }; + A100000100000000000004AB /* WebSocketClient.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = WebSocketClient.swift; sourceTree = ""; }; + A100000100000000000005AB /* Assets.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; path = Assets.xcassets; sourceTree = ""; }; + A100000100000000000006AB /* Info.plist */ = {isa = PBXFileReference; lastKnownFileType = text.plist.xml; path = Info.plist; sourceTree = ""; }; + A100000100000000000007AB /* SulTee.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = SulTee.app; sourceTree = BUILT_PRODUCTS_DIR; }; +/* End PBXFileReference section */ + +/* Begin PBXFrameworksBuildPhase section */ + A10000010000000000000FBP /* Frameworks */ = { + isa = PBXFrameworksBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXFrameworksBuildPhase section */ + +/* Begin PBXGroup section */ + A100000100000000000010GR /* / */ = { + isa = PBXGroup; + children = ( + A100000100000000000011GR /* SulTee */, + A100000100000000000012GR /* Products */, + ); + sourceTree = ""; + }; + A100000100000000000011GR /* SulTee */ = { + isa = PBXGroup; + children = ( + A100000100000000000001AB /* SulTeeApp.swift */, + A100000100000000000002AB /* ContentView.swift */, + A100000100000000000003AB /* SensorManager.swift */, + A100000100000000000004AB /* WebSocketClient.swift */, + A100000100000000000005AB /* Assets.xcassets */, + A100000100000000000006AB /* Info.plist */, + ); + path = SulTee; + sourceTree = ""; + }; + A100000100000000000012GR /* Products */ = { + isa = PBXGroup; + children = ( + A100000100000000000007AB /* SulTee.app */, + ); + name = Products; + sourceTree = ""; + }; +/* End PBXGroup section */ + +/* Begin PBXNativeTarget section */ + A100000100000000000001TG /* SulTee */ = { + isa = PBXNativeTarget; + buildConfigurationList = A100000100000000000001CL /* Build configuration list for PBXNativeTarget "SulTee" */; + buildPhases = ( + A10000010000000000000DBP /* Sources */, + A10000010000000000000EBP /* Resources */, + A10000010000000000000FBP /* Frameworks */, + ); + buildRules = ( + ); + dependencies = ( + ); + name = SulTee; + productName = SulTee; + productReference = A100000100000000000007AB /* SulTee.app */; + productType = "com.apple.product-type.application"; + }; +/* End PBXNativeTarget section */ + +/* Begin PBXProject section */ + A100000100000000000001PJ /* Project object */ = { + isa = PBXProject; + attributes = { + BuildIndependentTargetsInParallel = 1; + LastSwiftUpdateCheck = 1500; + LastUpgradeCheck = 1500; + ORGANIZATIONNAME = "SaltyLab"; + TargetAttributes = { + A100000100000000000001TG = { + CreatedOnToolsVersion = 15.0; + }; + }; + }; + buildConfigurationList = A100000100000000000002CL /* Build configuration list for PBXProject "SulTee" */; + compatibilityVersion = "Xcode 14.0"; + developmentRegion = en; + hasScannedForEncodings = 0; + knownRegions = ( + en, + Base, + ); + mainGroup = A100000100000000000010GR /* / */; + productRefGroup = A100000100000000000012GR /* Products */; + projectDirPath = ""; + projectRoot = ""; + targets = ( + A100000100000000000001TG /* SulTee */, + ); + }; +/* End PBXProject section */ + +/* Begin PBXResourcesBuildPhase section */ + A10000010000000000000EBP /* Resources */ = { + isa = PBXResourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + A100000100000000000005AA /* Assets.xcassets in Resources */, + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXResourcesBuildPhase section */ + +/* Begin PBXSourcesBuildPhase section */ + A10000010000000000000DBP /* Sources */ = { + isa = PBXSourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + A100000100000000000001AA /* SulTeeApp.swift in Sources */, + A100000100000000000002AA /* ContentView.swift in Sources */, + A100000100000000000003AA /* SensorManager.swift in Sources */, + A100000100000000000004AA /* WebSocketClient.swift in Sources */, + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXSourcesBuildPhase section */ + +/* Begin XCBuildConfiguration section */ + A100000100000000000001BC /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + ALWAYS_SEARCH_USER_PATHS = NO; + ASSETCATALOG_COMPILER_GENERATE_SWIFT_ASSET_SYMBOL_EXTENSIONS = YES; + CLANG_ANALYZER_NONNULL = YES; + CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE; + CLANG_CXX_LANGUAGE_STANDARD = "gnu++20"; + CLANG_ENABLE_MODULES = YES; + CLANG_ENABLE_OBJC_ARC = YES; + CLANG_ENABLE_OBJC_WEAK = YES; + CLANG_WARN_BLOCK_CAPTURE_AUTORELEASING = YES; + CLANG_WARN_BOOL_CONVERSION = YES; + CLANG_WARN_COMMA = YES; + CLANG_WARN_CONSTANT_CONVERSION = YES; + CLANG_WARN_DEPRECATED_OBJC_IMPLEMENTATIONS = YES; + CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR; + CLANG_WARN_DOCUMENTATION_COMMENTS = YES; + CLANG_WARN_EMPTY_BODY = YES; + CLANG_WARN_ENUM_CONVERSION = YES; + CLANG_WARN_INFINITE_RECURSION = YES; + CLANG_WARN_INT_CONVERSION = YES; + CLANG_WARN_NON_LITERAL_NULL_CONVERSION = YES; + CLANG_WARN_OBJC_IMPLICIT_RETAIN_CYCLE = YES; + CLANG_WARN_OBJC_LITERAL_CONVERSION = YES; + CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR; + CLANG_WARN_QUOTED_INCLUDE_IN_FRAMEWORK_HEADER = YES; + CLANG_WARN_RANGE_LOOP_ANALYSIS = YES; + CLANG_WARN_STRICT_PROTOTYPES = YES; + CLANG_WARN_SUSPICIOUS_MOVE = YES; + CLANG_WARN_UNGUARDED_AVAILABILITY = YES_AGGRESSIVE; + CLANG_WARN_UNREACHABLE_CODE = YES; + CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; + COPY_PHASE_STRIP = NO; + DEBUG_INFORMATION_FORMAT = dwarf; + ENABLE_STRICT_OBJC_MSGSEND = YES; + ENABLE_TESTABILITY = YES; + ENABLE_USER_SCRIPT_SANDBOXING = YES; + GCC_C_LANGUAGE_STANDARD = gnu17; + GCC_DYNAMIC_NO_PIC = NO; + GCC_NO_COMMON_BLOCKS = YES; + GCC_OPTIMIZATION_LEVEL = 0; + GCC_PREPROCESSOR_DEFINITIONS = ( + "DEBUG=1", + "$(inherited)", + ); + GCC_WARN_64_TO_32_BIT_CONVERSION = YES; + GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR; + GCC_WARN_UNDECLARED_SELECTOR = YES; + GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE; + GCC_WARN_UNUSED_FUNCTION = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + IPHONEOS_DEPLOYMENT_TARGET = 17.0; + MTL_ENABLE_DEBUG_INFO = INCLUDE_SOURCE; + MTL_FAST_MATH = YES; + ONLY_ACTIVE_ARCH = YES; + SDKROOT = iphoneos; + SWIFT_ACTIVE_COMPILATION_CONDITIONS = DEBUG; + SWIFT_OPTIMIZATION_LEVEL = "-Onone"; + }; + name = Debug; + }; + A100000100000000000002BC /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + ALWAYS_SEARCH_USER_PATHS = NO; + ASSETCATALOG_COMPILER_GENERATE_SWIFT_ASSET_SYMBOL_EXTENSIONS = YES; + CLANG_ANALYZER_NONNULL = YES; + CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE; + CLANG_CXX_LANGUAGE_STANDARD = "gnu++20"; + CLANG_ENABLE_MODULES = YES; + CLANG_ENABLE_OBJC_ARC = YES; + CLANG_ENABLE_OBJC_WEAK = YES; + CLANG_WARN_BLOCK_CAPTURE_AUTORELEASING = YES; + CLANG_WARN_BOOL_CONVERSION = YES; + CLANG_WARN_COMMA = YES; + CLANG_WARN_CONSTANT_CONVERSION = YES; + CLANG_WARN_DEPRECATED_OBJC_IMPLEMENTATIONS = YES; + CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR; + CLANG_WARN_DOCUMENTATION_COMMENTS = YES; + CLANG_WARN_EMPTY_BODY = YES; + CLANG_WARN_ENUM_CONVERSION = YES; + CLANG_WARN_INFINITE_RECURSION = YES; + CLANG_WARN_INT_CONVERSION = YES; + CLANG_WARN_NON_LITERAL_NULL_CONVERSION = YES; + CLANG_WARN_OBJC_IMPLICIT_RETAIN_CYCLE = YES; + CLANG_WARN_OBJC_LITERAL_CONVERSION = YES; + CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR; + CLANG_WARN_QUOTED_INCLUDE_IN_FRAMEWORK_HEADER = YES; + CLANG_WARN_RANGE_LOOP_ANALYSIS = YES; + CLANG_WARN_STRICT_PROTOTYPES = YES; + CLANG_WARN_SUSPICIOUS_MOVE = YES; + CLANG_WARN_UNGUARDED_AVAILABILITY = YES_AGGRESSIVE; + CLANG_WARN_UNREACHABLE_CODE = YES; + CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; + COPY_PHASE_STRIP = NO; + DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; + ENABLE_NS_ASSERTIONS = NO; + ENABLE_STRICT_OBJC_MSGSEND = YES; + ENABLE_USER_SCRIPT_SANDBOXING = YES; + GCC_C_LANGUAGE_STANDARD = gnu17; + GCC_NO_COMMON_BLOCKS = YES; + GCC_WARN_64_TO_32_BIT_CONVERSION = YES; + GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR; + GCC_WARN_UNDECLARED_SELECTOR = YES; + GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE; + GCC_WARN_UNUSED_FUNCTION = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + IPHONEOS_DEPLOYMENT_TARGET = 17.0; + MTL_FAST_MATH = YES; + SDKROOT = iphoneos; + SWIFT_COMPILATION_MODE = wholemodule; + SWIFT_OPTIMIZATION_LEVEL = "-O"; + VALIDATE_PRODUCT = YES; + }; + name = Release; + }; + A100000100000000000003BC /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon; + ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor; + CODE_SIGN_STYLE = Automatic; + CURRENT_PROJECT_VERSION = 1; + DEVELOPMENT_ASSET_PATHS = ""; + DEVELOPMENT_TEAM = Z37N597UWY; + ENABLE_PREVIEWS = YES; + GENERATE_INFOPLIST_FILE = NO; + INFOPLIST_FILE = SulTee/Info.plist; + IPHONEOS_DEPLOYMENT_TARGET = 17.0; + LD_RUNPATH_SEARCH_PATHS = ( + "$(inherited)", + "@executable_path/Frameworks", + ); + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.saltylab.sultee; + PRODUCT_NAME = "$(TARGET_NAME)"; + SWIFT_EMIT_LOC_STRINGS = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = 1; + }; + name = Debug; + }; + A100000100000000000004BC /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon; + ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor; + CODE_SIGN_STYLE = Automatic; + CURRENT_PROJECT_VERSION = 1; + DEVELOPMENT_ASSET_PATHS = ""; + DEVELOPMENT_TEAM = Z37N597UWY; + ENABLE_PREVIEWS = YES; + GENERATE_INFOPLIST_FILE = NO; + INFOPLIST_FILE = SulTee/Info.plist; + IPHONEOS_DEPLOYMENT_TARGET = 17.0; + LD_RUNPATH_SEARCH_PATHS = ( + "$(inherited)", + "@executable_path/Frameworks", + ); + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.saltylab.sultee; + PRODUCT_NAME = "$(TARGET_NAME)"; + SWIFT_EMIT_LOC_STRINGS = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = 1; + }; + name = Release; + }; +/* End XCBuildConfiguration section */ + +/* Begin XCConfigurationList section */ + A100000100000000000002CL /* Build configuration list for PBXProject "SulTee" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + A100000100000000000001BC /* Debug */, + A100000100000000000002BC /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; + A100000100000000000001CL /* Build configuration list for PBXNativeTarget "SulTee" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + A100000100000000000003BC /* Debug */, + A100000100000000000004BC /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; +/* End XCConfigurationList section */ + + }; + rootObject = A100000100000000000001PJ /* Project object */; +} diff --git a/SulTee/SulTee/Assets.xcassets/AppIcon.appiconset/Contents.json b/SulTee/SulTee/Assets.xcassets/AppIcon.appiconset/Contents.json new file mode 100644 index 0000000..13613e3 --- /dev/null +++ b/SulTee/SulTee/Assets.xcassets/AppIcon.appiconset/Contents.json @@ -0,0 +1,13 @@ +{ + "images" : [ + { + "idiom" : "universal", + "platform" : "ios", + "size" : "1024x1024" + } + ], + "info" : { + "author" : "xcode", + "version" : 1 + } +} diff --git a/SulTee/SulTee/Assets.xcassets/Contents.json b/SulTee/SulTee/Assets.xcassets/Contents.json new file mode 100644 index 0000000..73c0059 --- /dev/null +++ b/SulTee/SulTee/Assets.xcassets/Contents.json @@ -0,0 +1,6 @@ +{ + "info" : { + "author" : "xcode", + "version" : 1 + } +} diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift new file mode 100644 index 0000000..14741c8 --- /dev/null +++ b/SulTee/SulTee/ContentView.swift @@ -0,0 +1,115 @@ +import SwiftUI + +struct ContentView: View { + @EnvironmentObject var sensor: SensorManager + + var body: some View { + NavigationStack { + VStack(spacing: 24) { + connectionBanner + Divider() + sensorRatesGrid + Divider() + if let dist = sensor.botDistanceMeters { + Text("Bot distance: \(dist, specifier: "%.1f") m") + .font(.title2) + .bold() + } + Spacer() + followMeButton + } + .padding() + .navigationTitle("Sul-Tee") + } + } + + // MARK: - Subviews + + private var connectionBanner: some View { + HStack(spacing: 12) { + Circle() + .fill(wsColor) + .frame(width: 14, height: 14) + Text(wsLabel) + .font(.headline) + Spacer() + } + .padding(.top, 8) + } + + private var sensorRatesGrid: some View { + Grid(horizontalSpacing: 20, verticalSpacing: 12) { + GridRow { + rateCell(icon: "location.fill", label: "GPS", rate: sensor.gpsRate, unit: "Hz") + rateCell(icon: "gyroscope", label: "IMU", rate: sensor.imuRate, unit: "Hz") + } + GridRow { + rateCell(icon: "location.north.fill", label: "Heading", rate: sensor.headingRate, unit: "Hz") + rateCell(icon: "barometer", label: "Baro", rate: sensor.baroRate, unit: "Hz") + } + } + } + + private func rateCell(icon: String, label: String, rate: Double, unit: String) -> some View { + VStack(spacing: 4) { + Image(systemName: icon) + .font(.title2) + .foregroundStyle(rate > 0 ? .green : .secondary) + Text(label) + .font(.caption) + .foregroundStyle(.secondary) + Text("\(Int(rate)) \(unit)") + .font(.title3.monospacedDigit()) + .bold() + } + .frame(maxWidth: .infinity) + .padding() + .background(.quaternary, in: RoundedRectangle(cornerRadius: 12)) + } + + private var followMeButton: some View { + Button { + if sensor.isStreaming { + sensor.stopStreaming() + } else { + sensor.startStreaming() + } + } label: { + Label( + sensor.isStreaming ? "Stop Follow-Me" : "Start Follow-Me", + systemImage: sensor.isStreaming ? "stop.circle.fill" : "play.circle.fill" + ) + .font(.title2.bold()) + .frame(maxWidth: .infinity) + .padding() + .background(sensor.isStreaming ? Color.red : Color.accentColor, + in: RoundedRectangle(cornerRadius: 16)) + .foregroundStyle(.white) + } + .buttonStyle(.plain) + .padding(.bottom, 8) + } + + // MARK: - Helpers + + private var wsColor: Color { + switch sensor.wsState { + case .connected: return .green + case .connecting: return .yellow + case .disconnected: return .red + } + } + + private var wsLabel: String { + switch sensor.wsState { + case .connected: return "Connected — ws://192.168.86.158:9090" + case .connecting: return "Connecting…" + case .disconnected: return "Disconnected" + } + } +} + +#Preview { + ContentView() + .environmentObject(SensorManager()) +} diff --git a/SulTee/SulTee/Info.plist b/SulTee/SulTee/Info.plist new file mode 100644 index 0000000..1119bba --- /dev/null +++ b/SulTee/SulTee/Info.plist @@ -0,0 +1,57 @@ + + + + + CFBundleDevelopmentRegion + $(DEVELOPMENT_LANGUAGE) + CFBundleExecutable + $(EXECUTABLE_NAME) + CFBundleIdentifier + $(PRODUCT_BUNDLE_IDENTIFIER) + CFBundleInfoDictionaryVersion + 6.0 + CFBundleName + $(PRODUCT_NAME) + CFBundlePackageType + $(PRODUCT_BUNDLE_PACKAGE_TYPE) + CFBundleShortVersionString + 1.0 + CFBundleVersion + 1 + LSRequiresIPhoneOS + + NSLocationAlwaysAndWhenInUseUsageDescription + 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. + NSMotionUsageDescription + Sul-Tee streams IMU and barometer data to SaltyBot for follow-me stabilization. + UIBackgroundModes + + location + external-accessory + + UIDeviceFamily + + 1 + + UILaunchStoryboardName + LaunchScreen + UIRequiredDeviceCapabilities + + armv7 + + UISupportedInterfaceOrientations + + UIInterfaceOrientationPortrait + UIInterfaceOrientationPortraitUpsideDown + UIInterfaceOrientationLandscapeLeft + UIInterfaceOrientationLandscapeRight + + UIApplicationSceneManifest + + UIApplicationSupportsMultipleScenes + + + + diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift new file mode 100644 index 0000000..654cb77 --- /dev/null +++ b/SulTee/SulTee/SensorManager.swift @@ -0,0 +1,230 @@ +import Foundation +import CoreLocation +import CoreMotion +import Combine + +/// Manages all iPhone sensors and forwards data to the WebSocket client. +final class SensorManager: NSObject, ObservableObject { + + // MARK: - Published state for UI + + @Published var isStreaming = false + @Published var wsState: WebSocketClient.ConnectionState = .disconnected + @Published var gpsRate: Double = 0 + @Published var imuRate: Double = 0 + @Published var headingRate: Double = 0 + @Published var baroRate: Double = 0 + @Published var botDistanceMeters: Double? = nil + + // MARK: - Dependencies + + private let ws = WebSocketClient(url: URL(string: "ws://192.168.86.158:9090")!) + private let locationManager = CLLocationManager() + private let motionManager = CMMotionManager() + private let altimeter = CMAltimeter() + private var cancellables = Set() + + // MARK: - Rate counters (counts per second) + + private var gpsCounts: [Date] = [] + private var imuCounts: [Date] = [] + private var headingCounts: [Date] = [] + private var baroCounts: [Date] = [] + private var rateTimer: Timer? + + // MARK: - Init + + override init() { + super.init() + locationManager.delegate = self + locationManager.desiredAccuracy = kCLLocationAccuracyBestForNavigation + // Use dual-frequency GPS (L1+L5) on iPhone 15 Pro — automatic when accuracy is set to Best + locationManager.distanceFilter = kCLDistanceFilterNone + locationManager.allowsBackgroundLocationUpdates = true + locationManager.pausesLocationUpdatesAutomatically = false + locationManager.showsBackgroundLocationIndicator = true + + ws.$state + .receive(on: DispatchQueue.main) + .assign(to: \.wsState, on: self) + .store(in: &cancellables) + } + + // MARK: - Public control + + func startStreaming() { + guard !isStreaming else { return } + isStreaming = true + ws.connect() + requestPermissionsAndStartSensors() + startRateTimer() + } + + func stopStreaming() { + guard isStreaming else { return } + isStreaming = false + ws.disconnect() + stopSensors() + rateTimer?.invalidate() + rateTimer = nil + } + + // 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() + } + + private func startIMU() { + guard motionManager.isDeviceMotionAvailable else { return } + motionManager.deviceMotionUpdateInterval = 1.0 / 100.0 // 100 Hz + motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, error in + guard let self, let motion else { return } + self.recordEvent(in: &self.imuCounts) + let ts = Date().timeIntervalSince1970 + self.ws.send([ + "type": "imu", + "timestamp": ts, + "data": [ + "accel": [ + "x": motion.userAcceleration.x, + "y": motion.userAcceleration.y, + "z": motion.userAcceleration.z + ], + "gyro": [ + "x": motion.rotationRate.x, + "y": motion.rotationRate.y, + "z": motion.rotationRate.z + ], + "attitude": [ + "roll": motion.attitude.roll, + "pitch": motion.attitude.pitch, + "yaw": motion.attitude.yaw + ], + "gravity": [ + "x": motion.gravity.x, + "y": motion.gravity.y, + "z": motion.gravity.z + ], + "magneticField": [ + "x": motion.magneticField.field.x, + "y": motion.magneticField.field.y, + "z": motion.magneticField.field.z, + "accuracy": motion.magneticField.accuracy.rawValue + ] + ] + ]) + } + } + + private func startBarometer() { + guard CMAltimeter.isRelativeAltitudeAvailable() else { return } + altimeter.startRelativeAltitudeUpdates(to: .main) { [weak self] data, error in + guard let self, let data else { return } + self.recordEvent(in: &self.baroCounts) + self.ws.send([ + "type": "baro", + "timestamp": Date().timeIntervalSince1970, + "data": [ + "relativeAltitude": data.relativeAltitude.doubleValue, + "pressure": data.pressure.doubleValue + ] + ]) + } + } + + // MARK: - Rate tracking + + private func startRateTimer() { + rateTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in + self?.updateRates() + } + } + + private func recordEvent(in list: inout [Date]) { + list.append(Date()) + } + + private func updateRates() { + let cutoff = Date().addingTimeInterval(-1.0) + gpsCounts = gpsCounts.filter { $0 > cutoff } + imuCounts = imuCounts.filter { $0 > cutoff } + headingCounts = headingCounts.filter { $0 > cutoff } + baroCounts = baroCounts.filter { $0 > cutoff } + + DispatchQueue.main.async { + self.gpsRate = Double(self.gpsCounts.count) + self.imuRate = Double(self.imuCounts.count) + self.headingRate = Double(self.headingCounts.count) + self.baroRate = Double(self.baroCounts.count) + } + } +} + +// MARK: - CLLocationManagerDelegate + +extension SensorManager: CLLocationManagerDelegate { + + func locationManager(_ manager: CLLocationManager, didUpdateLocations locations: [CLLocation]) { + guard let loc = locations.last else { return } + recordEvent(in: &gpsCounts) + ws.send([ + "type": "gps", + "timestamp": loc.timestamp.timeIntervalSince1970, + "data": [ + "latitude": loc.coordinate.latitude, + "longitude": loc.coordinate.longitude, + "altitude": loc.altitude, + "horizontalAccuracy": loc.horizontalAccuracy, + "verticalAccuracy": loc.verticalAccuracy, + "speed": loc.speed, + "speedAccuracy": loc.speedAccuracy, + "course": loc.course, + "courseAccuracy": loc.courseAccuracy + ] + ]) + } + + func locationManager(_ manager: CLLocationManager, didUpdateHeading newHeading: CLHeading) { + recordEvent(in: &headingCounts) + ws.send([ + "type": "heading", + "timestamp": Date().timeIntervalSince1970, + "data": [ + "magneticHeading": newHeading.magneticHeading, + "trueHeading": newHeading.trueHeading, + "headingAccuracy": newHeading.headingAccuracy, + "x": newHeading.x, + "y": newHeading.y, + "z": newHeading.z + ] + ]) + } + + func locationManager(_ manager: CLLocationManager, didFailWithError error: Error) { + print("[Location] error: \(error)") + } + + func locationManagerDidChangeAuthorization(_ manager: CLLocationManager) { + switch manager.authorizationStatus { + case .authorizedAlways, .authorizedWhenInUse: + if isStreaming { + manager.startUpdatingLocation() + manager.startUpdatingHeading() + } + default: + break + } + } +} diff --git a/SulTee/SulTee/SulTeeApp.swift b/SulTee/SulTee/SulTeeApp.swift new file mode 100644 index 0000000..30738a7 --- /dev/null +++ b/SulTee/SulTee/SulTeeApp.swift @@ -0,0 +1,13 @@ +import SwiftUI + +@main +struct SulTeeApp: App { + @StateObject private var sensorManager = SensorManager() + + var body: some Scene { + WindowGroup { + ContentView() + .environmentObject(sensorManager) + } + } +} diff --git a/SulTee/SulTee/WebSocketClient.swift b/SulTee/SulTee/WebSocketClient.swift new file mode 100644 index 0000000..d898d5c --- /dev/null +++ b/SulTee/SulTee/WebSocketClient.swift @@ -0,0 +1,114 @@ +import Foundation +import UIKit + +/// Thin WebSocket wrapper around URLSessionWebSocketTask. +/// Reconnects automatically on disconnect. +final class WebSocketClient: NSObject, ObservableObject { + + enum ConnectionState { + case disconnected, connecting, connected + } + + @Published var state: ConnectionState = .disconnected + + private let url: URL + private var session: URLSession! + private var task: URLSessionWebSocketTask? + private var shouldRun = false + + init(url: URL) { + self.url = url + super.init() + self.session = URLSession(configuration: .default, delegate: self, delegateQueue: nil) + } + + func connect() { + shouldRun = true + guard state == .disconnected else { return } + openConnection() + } + + func disconnect() { + shouldRun = false + task?.cancel(with: .normalClosure, reason: nil) + task = nil + DispatchQueue.main.async { self.state = .disconnected } + } + + func send(_ message: [String: Any]) { + guard state == .connected, + let data = try? JSONSerialization.data(withJSONObject: message), + let json = String(data: data, encoding: .utf8) else { return } + + task?.send(.string(json)) { error in + if let error { + print("[WebSocket] send error: \(error)") + } + } + } + + // MARK: - Private + + private func openConnection() { + DispatchQueue.main.async { self.state = .connecting } + task = session.webSocketTask(with: url) + task?.resume() + scheduleReceive() + } + + private func scheduleReceive() { + task?.receive { [weak self] result in + guard let self else { return } + switch result { + case .success(let message): + self.handle(message) + self.scheduleReceive() + case .failure(let error): + print("[WebSocket] receive error: \(error)") + self.reconnectIfNeeded() + } + } + } + + private func handle(_ message: URLSessionWebSocketTask.Message) { + switch message { + case .string(let text): + guard let data = text.data(using: .utf8), + let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any], + let type = json["type"] as? String else { return } + + if type == "haptic" { + DispatchQueue.main.async { + UIImpactFeedbackGenerator(style: .medium).impactOccurred() + } + } + default: + break + } + } + + private func reconnectIfNeeded() { + DispatchQueue.main.async { self.state = .disconnected } + guard shouldRun else { return } + DispatchQueue.global().asyncAfter(deadline: .now() + 2) { [weak self] in + self?.openConnection() + } + } +} + +// MARK: - URLSessionWebSocketDelegate + +extension WebSocketClient: URLSessionWebSocketDelegate { + func urlSession(_ session: URLSession, + webSocketTask: URLSessionWebSocketTask, + didOpenWithProtocol protocol: String?) { + DispatchQueue.main.async { self.state = .connected } + } + + func urlSession(_ session: URLSession, + webSocketTask: URLSessionWebSocketTask, + didCloseWith closeCode: URLSessionWebSocketTask.CloseCode, + reason: Data?) { + reconnectIfNeeded() + } +} -- 2.47.2 From 52c9e823ac45d24ac9946fee1c45927d12c5a92b Mon Sep 17 00:00:00 2001 From: sl-ios Date: Fri, 3 Apr 2026 17:24:41 -0400 Subject: [PATCH 02/19] fix: switch WebSocket to Tailscale IP, add configurable Orin URL (Issue #709) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Default URL updated from ws://192.168.86.158:9090 (LAN) to ws://100.64.0.2:9090 (Tailscale) - URL persisted in UserDefaults under key "orinURL" — survives app restarts - WebSocketClient.url is now mutable so it can be updated without recreation - SensorManager.updateURL(_:) applies a new URL when not streaming - ContentView: editable text field for Orin address with Apply button, disabled while streaming - Connection banner shows the active URL instead of hardcoded string Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/ContentView.swift | 36 ++++++++++++++++++++++++++++- SulTee/SulTee/SensorManager.swift | 23 +++++++++++++++++- SulTee/SulTee/WebSocketClient.swift | 2 +- 3 files changed, 58 insertions(+), 3 deletions(-) diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift index 14741c8..c9b1ad0 100644 --- a/SulTee/SulTee/ContentView.swift +++ b/SulTee/SulTee/ContentView.swift @@ -2,11 +2,15 @@ import SwiftUI struct ContentView: View { @EnvironmentObject var sensor: SensorManager + @AppStorage("orinURL") private var orinURL: String = SensorManager.defaultOrinURL + @State private var editingURL: String = "" + @FocusState private var urlFieldFocused: Bool var body: some View { NavigationStack { VStack(spacing: 24) { connectionBanner + orinURLField Divider() sensorRatesGrid Divider() @@ -20,11 +24,41 @@ struct ContentView: View { } .padding() .navigationTitle("Sul-Tee") + .onAppear { editingURL = orinURL } } } // MARK: - Subviews + private var orinURLField: some View { + VStack(alignment: .leading, spacing: 4) { + Text("Orin WebSocket URL") + .font(.caption) + .foregroundStyle(.secondary) + HStack { + TextField("ws://host:port", text: $editingURL) + .textFieldStyle(.roundedBorder) + .keyboardType(.URL) + .autocorrectionDisabled() + .textInputAutocapitalization(.never) + .focused($urlFieldFocused) + .disabled(sensor.isStreaming) + .onSubmit { applyURL() } + if !sensor.isStreaming { + Button("Apply") { applyURL() } + .buttonStyle(.borderedProminent) + .controlSize(.small) + } + } + } + } + + private func applyURL() { + urlFieldFocused = false + orinURL = editingURL + sensor.updateURL(editingURL) + } + private var connectionBanner: some View { HStack(spacing: 12) { Circle() @@ -102,7 +136,7 @@ struct ContentView: View { private var wsLabel: String { switch sensor.wsState { - case .connected: return "Connected — ws://192.168.86.158:9090" + case .connected: return "Connected — \(orinURL)" case .connecting: return "Connecting…" case .disconnected: return "Disconnected" } diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 654cb77..70531e2 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -18,7 +18,16 @@ final class SensorManager: NSObject, ObservableObject { // MARK: - Dependencies - private let ws = WebSocketClient(url: URL(string: "ws://192.168.86.158:9090")!) + static let defaultOrinURL = "ws://100.64.0.2:9090" + private static let orinURLKey = "orinURL" + + private(set) var ws: WebSocketClient + + /// Current Orin WebSocket URL string (persisted in UserDefaults). + var orinURLString: String { + get { UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL } + set { UserDefaults.standard.set(newValue, forKey: Self.orinURLKey) } + } private let locationManager = CLLocationManager() private let motionManager = CMMotionManager() private let altimeter = CMAltimeter() @@ -35,6 +44,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)!) super.init() locationManager.delegate = self locationManager.desiredAccuracy = kCLLocationAccuracyBestForNavigation @@ -69,6 +80,16 @@ final class SensorManager: NSObject, ObservableObject { rateTimer = nil } + /// Call when the user edits the Orin URL. Persists the value and updates + /// the client URL; takes effect on the next connect(). + func updateURL(_ urlString: String) { + guard !isStreaming else { return } + orinURLString = urlString + if let url = URL(string: urlString), url.scheme?.hasPrefix("ws") == true { + ws.url = url + } + } + // MARK: - Sensor start / stop private func requestPermissionsAndStartSensors() { diff --git a/SulTee/SulTee/WebSocketClient.swift b/SulTee/SulTee/WebSocketClient.swift index d898d5c..5c8e2e9 100644 --- a/SulTee/SulTee/WebSocketClient.swift +++ b/SulTee/SulTee/WebSocketClient.swift @@ -11,7 +11,7 @@ final class WebSocketClient: NSObject, ObservableObject { @Published var state: ConnectionState = .disconnected - private let url: URL + var url: URL private var session: URLSession! private var task: URLSessionWebSocketTask? private var shouldRun = false -- 2.47.2 From a7a4ed262ac0d42773427d80a95448728703ae93 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Fri, 3 Apr 2026 18:02:18 -0400 Subject: [PATCH 03/19] fix: add ATS exception for Tailscale WebSocket (error -1022) (Issue #709) NSAllowsLocalNetworking covers CGNAT 100.64.0.0/10 range used by Tailscale, fixing ATS blocking plain ws:// connections. Also adds NSExceptionDomains entry for 100.64.0.2 as explicit fallback. Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/Info.plist | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/SulTee/SulTee/Info.plist b/SulTee/SulTee/Info.plist index 1119bba..40874c4 100644 --- a/SulTee/SulTee/Info.plist +++ b/SulTee/SulTee/Info.plist @@ -48,6 +48,22 @@ UIInterfaceOrientationLandscapeLeft UIInterfaceOrientationLandscapeRight + NSAppTransportSecurity + + + NSAllowsLocalNetworking + + NSExceptionDomains + + 100.64.0.2 + + NSExceptionAllowsInsecureHTTPLoads + + + + UIApplicationSceneManifest UIApplicationSupportsMultipleScenes -- 2.47.2 From f39b9d432dab6d23621ab60f9a1156a9e7e5d8e5 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Sat, 4 Apr 2026 11:11:11 -0400 Subject: [PATCH 04/19] feat: publish iOS GPS to MQTT topic saltybot/ios/gps at 1 Hz (Issue #681) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds minimal MQTT 3.1.1 client (MQTTClient.swift) using Network.framework — no external dependency. Implements CONNECT + PUBLISH (QoS 0) + PINGREQ keepalive. - Broker: 192.168.87.29:1883 (user: mqtt_seb) - Topic: saltybot/ios/gps - Rate: 1 Hz Timer, decoupled from GPS update rate - Payload matches sensor_dashboard.py format: {ts, lat, lon, alt_m, accuracy_m, speed_ms, bearing_deg, provider: "gps"} - lastKnownLocation cached from CLLocationManagerDelegate, published on timer - MQTT connect/disconnect tied to startStreaming()/stopStreaming() - ATS NSExceptionDomains extended to include 192.168.87.29 (MQTT broker LAN IP) - MQTTClient.swift registered in project.pbxproj Sources build phase Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee.xcodeproj/project.pbxproj | 4 + SulTee/SulTee/Info.plist | 5 + SulTee/SulTee/MQTTClient.swift | 187 ++++++++++++++++++++++++ SulTee/SulTee/SensorManager.swift | 52 ++++++- 4 files changed, 246 insertions(+), 2 deletions(-) create mode 100644 SulTee/SulTee/MQTTClient.swift diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index c0193ec..c3a2c72 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -12,6 +12,7 @@ A100000100000000000003AA /* SensorManager.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000003AB /* SensorManager.swift */; }; A100000100000000000004AA /* WebSocketClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000004AB /* WebSocketClient.swift */; }; A100000100000000000005AA /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = A100000100000000000005AB /* Assets.xcassets */; }; + A100000100000000000009AA /* MQTTClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000009AB /* MQTTClient.swift */; }; /* End PBXBuildFile section */ /* Begin PBXFileReference section */ @@ -22,6 +23,7 @@ A100000100000000000005AB /* Assets.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; path = Assets.xcassets; sourceTree = ""; }; A100000100000000000006AB /* Info.plist */ = {isa = PBXFileReference; lastKnownFileType = text.plist.xml; path = Info.plist; sourceTree = ""; }; 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 = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ @@ -50,6 +52,7 @@ A100000100000000000002AB /* ContentView.swift */, A100000100000000000003AB /* SensorManager.swift */, A100000100000000000004AB /* WebSocketClient.swift */, + A100000100000000000009AB /* MQTTClient.swift */, A100000100000000000005AB /* Assets.xcassets */, A100000100000000000006AB /* Info.plist */, ); @@ -138,6 +141,7 @@ A100000100000000000002AA /* ContentView.swift in Sources */, A100000100000000000003AA /* SensorManager.swift in Sources */, A100000100000000000004AA /* WebSocketClient.swift in Sources */, + A100000100000000000009AA /* MQTTClient.swift in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/SulTee/SulTee/Info.plist b/SulTee/SulTee/Info.plist index 40874c4..8e3361b 100644 --- a/SulTee/SulTee/Info.plist +++ b/SulTee/SulTee/Info.plist @@ -62,6 +62,11 @@ NSExceptionAllowsInsecureHTTPLoads + 192.168.87.29 + + NSExceptionAllowsInsecureHTTPLoads + + UIApplicationSceneManifest diff --git a/SulTee/SulTee/MQTTClient.swift b/SulTee/SulTee/MQTTClient.swift new file mode 100644 index 0000000..88df9bc --- /dev/null +++ b/SulTee/SulTee/MQTTClient.swift @@ -0,0 +1,187 @@ +import Foundation +import Network + +/// Minimal MQTT 3.1.1 client — CONNECT + PUBLISH (QoS 0) + PINGREQ only. +/// Sufficient for 1 Hz telemetry publishing; no subscription support needed. +final class MQTTClient { + + struct Config { + var host: String + var port: UInt16 + var username: String + var password: String + var clientID: String + var keepAlive: UInt16 = 60 + } + + enum State { case disconnected, connecting, connected } + + private(set) var state: State = .disconnected + private var config: Config + private var connection: NWConnection? + private var pingTimer: DispatchSourceTimer? + private var shouldRun = false + private let queue = DispatchQueue(label: "mqtt.client", qos: .utility) + + init(config: Config) { + self.config = config + } + + // MARK: - Public + + func connect() { + shouldRun = true + guard state == .disconnected else { return } + openConnection() + } + + func disconnect() { + shouldRun = false + pingTimer?.cancel() + pingTimer = nil + connection?.cancel() + connection = nil + state = .disconnected + } + + /// Publish a UTF-8 string payload to `topic` at QoS 0. + func publish(topic: String, payload: String) { + guard state == .connected else { return } + let packet = buildPublish(topic: topic, payload: payload) + connection?.send(content: packet, completion: .idempotent) + } + + // MARK: - Connection lifecycle + + private func openConnection() { + state = .connecting + let host = NWEndpoint.Host(config.host) + let port = NWEndpoint.Port(rawValue: config.port)! + connection = NWConnection(host: host, port: port, using: .tcp) + connection?.stateUpdateHandler = { [weak self] newState in + self?.handleStateChange(newState) + } + connection?.start(queue: queue) + scheduleRead() + } + + private func handleStateChange(_ newState: NWConnection.State) { + switch newState { + case .ready: + sendConnect() + schedulePing() + case .failed, .cancelled: + state = .disconnected + pingTimer?.cancel() + pingTimer = nil + reconnectIfNeeded() + default: + break + } + } + + private func reconnectIfNeeded() { + guard shouldRun else { return } + queue.asyncAfter(deadline: .now() + 3) { [weak self] in + self?.openConnection() + } + } + + // MARK: - Read loop (to receive CONNACK / PINGRESP) + + private func scheduleRead() { + connection?.receive(minimumIncompleteLength: 2, maximumLength: 256) { [weak self] data, _, _, error in + guard let self else { return } + if let data, !data.isEmpty { + self.handleIncoming(data) + } + if error == nil { self.scheduleRead() } + } + } + + private func handleIncoming(_ data: Data) { + guard let first = data.first else { return } + switch first { + case 0x20: // CONNACK + state = .connected + case 0xD0: // PINGRESP — no action needed + break + default: + break + } + } + + // MARK: - Keep-alive ping + + private func schedulePing() { + let t = DispatchSource.makeTimerSource(queue: queue) + t.schedule(deadline: .now() + Double(config.keepAlive / 2), + repeating: Double(config.keepAlive / 2)) + t.setEventHandler { [weak self] in + self?.sendPing() + } + t.resume() + pingTimer = t + } + + private func sendPing() { + let packet = Data([0xC0, 0x00]) + connection?.send(content: packet, completion: .idempotent) + } + + // MARK: - MQTT packet builders + + private func sendConnect() { + let packet = buildConnect() + connection?.send(content: packet, completion: .idempotent) + } + + private func buildConnect() -> Data { + var payload = Data() + payload += mqttString("MQTT") // protocol name + payload.append(0x04) // protocol level (3.1.1) + payload.append(0xC2) // flags: username + password + clean session + payload += uint16BE(config.keepAlive) // keep-alive + payload += mqttString(config.clientID) // client ID + payload += mqttString(config.username) // username + payload += mqttString(config.password) // password + return mqttPacket(type: 0x10, payload: payload) + } + + private func buildPublish(topic: String, payload: String) -> Data { + var body = Data() + body += mqttString(topic) + body += payload.data(using: .utf8) ?? Data() + return mqttPacket(type: 0x30, payload: body) + } + + // MARK: - Encoding helpers + + private func mqttPacket(type: UInt8, payload: Data) -> Data { + var packet = Data([type]) + packet += remainingLength(payload.count) + packet += payload + return packet + } + + private func remainingLength(_ value: Int) -> Data { + var data = Data() + var n = value + repeat { + var byte = UInt8(n & 0x7F) + n >>= 7 + if n > 0 { byte |= 0x80 } + data.append(byte) + } while n > 0 + return data + } + + private func mqttString(_ s: String) -> Data { + let bytes = s.data(using: .utf8) ?? Data() + return uint16BE(UInt16(bytes.count)) + bytes + } + + private func uint16BE(_ v: UInt16) -> Data { + Data([UInt8(v >> 8), UInt8(v & 0xFF)]) + } +} diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 70531e2..9604f0a 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -3,7 +3,8 @@ import CoreLocation import CoreMotion import Combine -/// Manages all iPhone sensors and forwards data to the WebSocket client. +/// Manages all iPhone sensors and forwards data to the WebSocket client +/// and MQTT broker (topic: saltybot/ios/gps, 1 Hz). final class SensorManager: NSObject, ObservableObject { // MARK: - Published state for UI @@ -16,7 +17,7 @@ final class SensorManager: NSObject, ObservableObject { @Published var baroRate: Double = 0 @Published var botDistanceMeters: Double? = nil - // MARK: - Dependencies + // MARK: - WebSocket static let defaultOrinURL = "ws://100.64.0.2:9090" private static let orinURLKey = "orinURL" @@ -28,6 +29,22 @@ final class SensorManager: NSObject, ObservableObject { get { UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL } set { UserDefaults.standard.set(newValue, forKey: Self.orinURLKey) } } + + // MARK: - MQTT + + private let mqtt = MQTTClient(config: .init( + host: "192.168.87.29", + port: 1883, + username: "mqtt_seb", + password: "mqtt_pass", + clientID: "sultee-ios-\(UUID().uuidString.prefix(8))" + )) + private static let mqttGPSTopic = "saltybot/ios/gps" + private var lastKnownLocation: CLLocation? + private var mqttPublishTimer: Timer? + + // MARK: - Sensors + private let locationManager = CLLocationManager() private let motionManager = CMMotionManager() private let altimeter = CMAltimeter() @@ -67,17 +84,22 @@ final class SensorManager: NSObject, ObservableObject { guard !isStreaming else { return } isStreaming = true ws.connect() + mqtt.connect() requestPermissionsAndStartSensors() startRateTimer() + startMQTTPublishTimer() } func stopStreaming() { guard isStreaming else { return } isStreaming = false ws.disconnect() + mqtt.disconnect() stopSensors() rateTimer?.invalidate() rateTimer = nil + mqttPublishTimer?.invalidate() + mqttPublishTimer = nil } /// Call when the user edits the Orin URL. Persists the value and updates @@ -90,6 +112,31 @@ final class SensorManager: NSObject, ObservableObject { } } + // MARK: - MQTT GPS publish (1 Hz) + + private func startMQTTPublishTimer() { + mqttPublishTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in + self?.publishGPSToMQTT() + } + } + + private func publishGPSToMQTT() { + guard let loc = lastKnownLocation else { return } + let payload: [String: Any] = [ + "ts": loc.timestamp.timeIntervalSince1970, + "lat": loc.coordinate.latitude, + "lon": loc.coordinate.longitude, + "alt_m": loc.altitude, + "accuracy_m": max(0, loc.horizontalAccuracy), + "speed_ms": max(0, loc.speed), + "bearing_deg": loc.course >= 0 ? loc.course : 0.0, + "provider": "gps" + ] + guard let data = try? JSONSerialization.data(withJSONObject: payload), + let json = String(data: data, encoding: .utf8) else { return } + mqtt.publish(topic: Self.mqttGPSTopic, payload: json) + } + // MARK: - Sensor start / stop private func requestPermissionsAndStartSensors() { @@ -199,6 +246,7 @@ extension SensorManager: CLLocationManagerDelegate { func locationManager(_ manager: CLLocationManager, didUpdateLocations locations: [CLLocation]) { guard let loc = locations.last else { return } + lastKnownLocation = loc recordEvent(in: &gpsCounts) ws.send([ "type": "gps", -- 2.47.2 From 0ad2b2f5c037f4d9da666d11b1fde0af9789b11a Mon Sep 17 00:00:00 2001 From: sl-ios Date: Sat, 4 Apr 2026 11:41:11 -0400 Subject: [PATCH 05/19] feat: Rename to SAUL-T-MOTE, add map with user + robot positions and follow path MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rename: - CFBundleDisplayName = "SAUL-T-MOTE" in Info.plist - navigationTitle updated to "SAUL-T-MOTE" in StatusView - MQTT clientID prefix changed to "saul-t-mote-" Map view (MapContentView.swift, MapKit): - Blue marker + fading breadcrumb trail for user (iPhone GPS) - Orange car marker + fading breadcrumb trail for robot (Pixel 5) - Dashed yellow line from robot → user (follow path) - Bottom overlay: distance between user and robot, robot speed - Auto-follow camera tracks user; manual drag disables it; re-centre button restores - MapPolyline for trails, per-point Annotation for fading breadcrumb dots Robot GPS subscription (saltybot/phone/gps): - MQTTClient extended with SUBSCRIBE (QoS 0) + incoming PUBLISH parser (handles variable-length remaining-length, multi-packet frames) - Subscriptions persisted and re-sent on reconnect (CONNACK handler) - SensorManager.handleRobotGPS() updates robotLocation, robotSpeed, robotBreadcrumbs, distanceToRobot iOS GPS publish unchanged (saltybot/ios/gps, 1 Hz) — PR #2 intact. ContentView restructured as TabView: - Tab 1: Status (sensor rates, WS URL, follow-me button) - Tab 2: Map Co-Authored-By: Claude Sonnet 4.6 --- CLAUDE.md | 67 ++++++ SulTee/SulTee.xcodeproj/project.pbxproj | 4 + .../contents.xcworkspacedata | 7 + SulTee/SulTee/ContentView.swift | 87 +++++--- SulTee/SulTee/Info.plist | 2 + SulTee/SulTee/MQTTClient.swift | 137 ++++++++---- SulTee/SulTee/MapContentView.swift | 184 ++++++++++++++++ SulTee/SulTee/SensorManager.swift | 196 ++++++++++-------- 8 files changed, 531 insertions(+), 153 deletions(-) create mode 100644 CLAUDE.md create mode 100644 SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata create mode 100644 SulTee/SulTee/MapContentView.swift diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..3fb1c23 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,67 @@ +# sl-ios — iOS Companion App Agent (Sul-Tee) + +## Role +You are sl-ios, a SaltyLab agent building the iOS companion app ("Sul-Tee") for SaltyBot follow-me mode. The app runs on iPhone 15 Pro and streams GPS, IMU, magnetometer, and barometer data over WebSocket to the Jetson Orin. + +## Scope +- Swift / SwiftUI native iOS app (iOS 17+, iPhone 15 Pro target) +- CoreLocation (dual-frequency GPS L1+L5), CoreMotion (IMU, mag, baro) +- WebSocket client streaming sensor data to Orin +- Background operation (must keep streaming when phone locked/backgrounded) +- Simple status UI: connection state, sensor rates, bot distance +- Start/stop follow-me button, haptic alerts from bot +- Future: LiDAR depth data passthrough + +## Tech Stack +- **Language:** Swift +- **UI:** SwiftUI +- **Sensors:** CoreLocation, CoreMotion, CMAltimeter +- **Networking:** URLSessionWebSocketTask (native WebSocket) +- **Protocol:** JSON over WebSocket (binary optimization later) +- **Target:** iPhone 15 Pro, iOS 17+ +- **Xcode dev account:** vayrette@gmail.com (team Z37N597UWY) + +## Architecture Context +- UWB ranging is handled by ESP32 DW1000 anchors on the bot (NOT Apple U1) +- iPhone provides GPS + IMU + mag + baro over WiFi/WebSocket to Orin +- Orin fuses phone sensors + UWB ranges for position estimate +- Orin IP: 192.168.86.158 (saltylab-orin) + +## Repository +- Repo: `seb/saltylab-ios` on gitea.vayrette.com +- Target branch: `origin/main` +- PR login: `--login sl-ios` (via tea CLI, or sl-jetson if no access) + +## Git Rules (MANDATORY) +1. Always rebase before starting: `git fetch origin && git rebase origin/main` +2. Always rebase before pushing: `git fetch origin && git rebase origin/main` +3. Branch naming: `sl-ios/issue--` + +## MQTT Communication +```bash +# Send message to max (PM): +AGENT_NAME=sl-ios ~/agent-mqtt/agent-send max "your message" + +# Read inbox: +~/agent-mqtt/agent-read 2>/dev/null | tail -15 +``` +Prioritize messages from max in your inbox. + +## PR Workflow +```bash +tea pr create --login sl-ios --repo seb/saltylab-ios \ + --title 'feat: (Issue #N)' \ + --description '
' --base main +``` +If push fails (permission denied), report via MQTT — sl-jetson will push for you. + +## Tab Naming +Update iTerm tab to reflect state: +- Working: `printf '\e]1;%s\a' "sl-ios - issue-"` +- Done: `printf '\e]1;%s\a' "sl-ios - reported to max"` +- Idle: `printf '\e]1;%s\a' "sl-ios - idle"` + +## Reference +- Issue #709: https://gitea.vayrette.com/seb/saltylab-firmware/issues/709 +- UWB firmware branch: `salty/uwb-tag-display-wireless` +- SaltyBot architecture: see saltylab-firmware repo docs diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index c3a2c72..1a6ff66 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -13,6 +13,7 @@ A100000100000000000004AA /* WebSocketClient.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000004AB /* WebSocketClient.swift */; }; 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 */; }; /* End PBXBuildFile section */ /* Begin PBXFileReference section */ @@ -24,6 +25,7 @@ A100000100000000000006AB /* Info.plist */ = {isa = PBXFileReference; lastKnownFileType = text.plist.xml; path = Info.plist; sourceTree = ""; }; 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 = ""; }; + A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ @@ -53,6 +55,7 @@ A100000100000000000003AB /* SensorManager.swift */, A100000100000000000004AB /* WebSocketClient.swift */, A100000100000000000009AB /* MQTTClient.swift */, + A10000010000000000000AAB /* MapContentView.swift */, A100000100000000000005AB /* Assets.xcassets */, A100000100000000000006AB /* Info.plist */, ); @@ -142,6 +145,7 @@ A100000100000000000003AA /* SensorManager.swift in Sources */, A100000100000000000004AA /* WebSocketClient.swift in Sources */, A100000100000000000009AA /* MQTTClient.swift in Sources */, + A10000010000000000000AAA /* MapContentView.swift in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata b/SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata new file mode 100644 index 0000000..919434a --- /dev/null +++ b/SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata @@ -0,0 +1,7 @@ + + + + + diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift index c9b1ad0..6ca50da 100644 --- a/SulTee/SulTee/ContentView.swift +++ b/SulTee/SulTee/ContentView.swift @@ -1,7 +1,24 @@ import SwiftUI +// MARK: - Root (tab container) + struct ContentView: View { @EnvironmentObject var sensor: SensorManager + + var body: some View { + TabView { + StatusView() + .tabItem { Label("Status", systemImage: "antenna.radiowaves.left.and.right") } + MapContentView() + .tabItem { Label("Map", systemImage: "map.fill") } + } + } +} + +// MARK: - Status tab + +private struct StatusView: View { + @EnvironmentObject var sensor: SensorManager @AppStorage("orinURL") private var orinURL: String = SensorManager.defaultOrinURL @State private var editingURL: String = "" @FocusState private var urlFieldFocused: Bool @@ -14,21 +31,33 @@ struct ContentView: View { Divider() sensorRatesGrid Divider() - if let dist = sensor.botDistanceMeters { - Text("Bot distance: \(dist, specifier: "%.1f") m") - .font(.title2) - .bold() + if let dist = sensor.distanceToRobot { + distanceRow(dist) } Spacer() followMeButton } .padding() - .navigationTitle("Sul-Tee") + .navigationTitle("SAUL-T-MOTE") .onAppear { editingURL = orinURL } } } - // MARK: - Subviews + // MARK: Connection banner + + private var connectionBanner: some View { + HStack(spacing: 12) { + Circle() + .fill(wsColor) + .frame(width: 14, height: 14) + Text(wsLabel) + .font(.headline) + Spacer() + } + .padding(.top, 8) + } + + // MARK: Orin URL field private var orinURLField: some View { VStack(alignment: .leading, spacing: 4) { @@ -59,32 +88,22 @@ struct ContentView: View { sensor.updateURL(editingURL) } - private var connectionBanner: some View { - HStack(spacing: 12) { - Circle() - .fill(wsColor) - .frame(width: 14, height: 14) - Text(wsLabel) - .font(.headline) - Spacer() - } - .padding(.top, 8) - } + // MARK: Sensor rates grid private var sensorRatesGrid: some View { Grid(horizontalSpacing: 20, verticalSpacing: 12) { GridRow { - rateCell(icon: "location.fill", label: "GPS", rate: sensor.gpsRate, unit: "Hz") - rateCell(icon: "gyroscope", label: "IMU", rate: sensor.imuRate, unit: "Hz") + rateCell(icon: "location.fill", label: "GPS", rate: sensor.gpsRate) + rateCell(icon: "gyroscope", label: "IMU", rate: sensor.imuRate) } GridRow { - rateCell(icon: "location.north.fill", label: "Heading", rate: sensor.headingRate, unit: "Hz") - rateCell(icon: "barometer", label: "Baro", rate: sensor.baroRate, unit: "Hz") + rateCell(icon: "location.north.fill", label: "Heading", rate: sensor.headingRate) + rateCell(icon: "barometer", label: "Baro", rate: sensor.baroRate) } } } - private func rateCell(icon: String, label: String, rate: Double, unit: String) -> some View { + private func rateCell(icon: String, label: String, rate: Double) -> some View { VStack(spacing: 4) { Image(systemName: icon) .font(.title2) @@ -92,7 +111,7 @@ struct ContentView: View { Text(label) .font(.caption) .foregroundStyle(.secondary) - Text("\(Int(rate)) \(unit)") + Text("\(Int(rate)) Hz") .font(.title3.monospacedDigit()) .bold() } @@ -101,13 +120,23 @@ struct ContentView: View { .background(.quaternary, in: RoundedRectangle(cornerRadius: 12)) } + // 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() + } + } + + // MARK: Follow-Me button + private var followMeButton: some View { Button { - if sensor.isStreaming { - sensor.stopStreaming() - } else { - sensor.startStreaming() - } + sensor.isStreaming ? sensor.stopStreaming() : sensor.startStreaming() } label: { Label( sensor.isStreaming ? "Stop Follow-Me" : "Start Follow-Me", @@ -124,7 +153,7 @@ struct ContentView: View { .padding(.bottom, 8) } - // MARK: - Helpers + // MARK: Helpers private var wsColor: Color { switch sensor.wsState { diff --git a/SulTee/SulTee/Info.plist b/SulTee/SulTee/Info.plist index 8e3361b..d00c04b 100644 --- a/SulTee/SulTee/Info.plist +++ b/SulTee/SulTee/Info.plist @@ -10,6 +10,8 @@ $(PRODUCT_BUNDLE_IDENTIFIER) CFBundleInfoDictionaryVersion 6.0 + CFBundleDisplayName + SAUL-T-MOTE CFBundleName $(PRODUCT_NAME) CFBundlePackageType diff --git a/SulTee/SulTee/MQTTClient.swift b/SulTee/SulTee/MQTTClient.swift index 88df9bc..8072714 100644 --- a/SulTee/SulTee/MQTTClient.swift +++ b/SulTee/SulTee/MQTTClient.swift @@ -1,8 +1,8 @@ import Foundation import Network -/// Minimal MQTT 3.1.1 client — CONNECT + PUBLISH (QoS 0) + PINGREQ only. -/// Sufficient for 1 Hz telemetry publishing; no subscription support needed. +/// Minimal MQTT 3.1.1 client — CONNECT + PUBLISH (QoS 0) + SUBSCRIBE (QoS 0) + PINGREQ. +/// Supports both publish and subscribe; no QoS 1/2 needed for this use-case. final class MQTTClient { struct Config { @@ -17,10 +17,16 @@ final class MQTTClient { enum State { case disconnected, connecting, connected } private(set) var state: State = .disconnected + + /// Called on the main queue for every received PUBLISH message: (topic, payload). + var onMessage: ((String, String) -> Void)? + private var config: Config private var connection: NWConnection? private var pingTimer: DispatchSourceTimer? private var shouldRun = false + private var subscriptions: [String] = [] // persisted across reconnects + private var nextPacketID: UInt16 = 1 private let queue = DispatchQueue(label: "mqtt.client", qos: .utility) init(config: Config) { @@ -47,21 +53,27 @@ final class MQTTClient { /// Publish a UTF-8 string payload to `topic` at QoS 0. func publish(topic: String, payload: String) { guard state == .connected else { return } - let packet = buildPublish(topic: topic, payload: payload) - connection?.send(content: packet, completion: .idempotent) + connection?.send(content: buildPublish(topic: topic, payload: payload), + completion: .idempotent) + } + + /// Subscribe to `topic` at QoS 0. Stored and re-sent automatically on reconnect. + func subscribe(topic: String) { + if !subscriptions.contains(topic) { subscriptions.append(topic) } + guard state == .connected else { return } + sendSubscribe(topic: topic) } // MARK: - Connection lifecycle private func openConnection() { state = .connecting - let host = NWEndpoint.Host(config.host) - let port = NWEndpoint.Port(rawValue: config.port)! - connection = NWConnection(host: host, port: port, using: .tcp) - connection?.stateUpdateHandler = { [weak self] newState in - self?.handleStateChange(newState) - } - connection?.start(queue: queue) + let conn = NWConnection(host: NWEndpoint.Host(config.host), + port: NWEndpoint.Port(rawValue: config.port)!, + using: .tcp) + conn.stateUpdateHandler = { [weak self] s in self?.handleStateChange(s) } + conn.start(queue: queue) + connection = conn scheduleRead() } @@ -82,32 +94,69 @@ final class MQTTClient { private func reconnectIfNeeded() { guard shouldRun else { return } - queue.asyncAfter(deadline: .now() + 3) { [weak self] in - self?.openConnection() - } + queue.asyncAfter(deadline: .now() + 3) { [weak self] in self?.openConnection() } } - // MARK: - Read loop (to receive CONNACK / PINGRESP) + // MARK: - Read loop private func scheduleRead() { - connection?.receive(minimumIncompleteLength: 2, maximumLength: 256) { [weak self] data, _, _, error in + connection?.receive(minimumIncompleteLength: 2, maximumLength: 4096) { [weak self] data, _, _, error in guard let self else { return } - if let data, !data.isEmpty { - self.handleIncoming(data) - } + if let data, !data.isEmpty { self.handleIncoming(data) } if error == nil { self.scheduleRead() } } } + /// Parse one or more MQTT packets from `data`. private func handleIncoming(_ data: Data) { - guard let first = data.first else { return } - switch first { - case 0x20: // CONNACK - state = .connected - case 0xD0: // PINGRESP — no action needed - break - default: - break + var i = data.startIndex + + while i < data.endIndex { + let firstByte = data[i] + i = data.index(after: i) + let packetType = firstByte & 0xF0 + + // Decode variable-length remaining-length field + var multiplier = 1 + var remaining = 0 + var lenByte: UInt8 = 0 + repeat { + guard i < data.endIndex else { return } + lenByte = data[i] + i = data.index(after: i) + remaining += Int(lenByte & 0x7F) * multiplier + multiplier *= 128 + } while lenByte & 0x80 != 0 + + guard let payloadEnd = data.index(i, offsetBy: remaining, limitedBy: data.endIndex) else { break } + + switch packetType { + case 0x20: // CONNACK + state = .connected + for topic in subscriptions { sendSubscribe(topic: topic) } + + case 0x30: // PUBLISH (QoS 0 — no packet identifier) + var j = i + if data.distance(from: j, to: payloadEnd) >= 2 { + let topicLen = Int(data[j]) << 8 | Int(data[data.index(after: j)]) + j = data.index(j, offsetBy: 2) + if let topicEnd = data.index(j, offsetBy: topicLen, limitedBy: payloadEnd) { + let topic = String(bytes: data[j.. Data { var payload = Data() - payload += mqttString("MQTT") // protocol name - payload.append(0x04) // protocol level (3.1.1) - payload.append(0xC2) // flags: username + password + clean session - payload += uint16BE(config.keepAlive) // keep-alive - payload += mqttString(config.clientID) // client ID - payload += mqttString(config.username) // username - payload += mqttString(config.password) // password + payload += mqttString("MQTT") + payload.append(0x04) // protocol level 3.1.1 + payload.append(0xC2) // flags: username + password + clean session + payload += uint16BE(config.keepAlive) + payload += mqttString(config.clientID) + payload += mqttString(config.username) + payload += mqttString(config.password) return mqttPacket(type: 0x10, payload: payload) } @@ -155,6 +198,16 @@ final class MQTTClient { return mqttPacket(type: 0x30, payload: body) } + private func sendSubscribe(topic: String) { + var payload = Data() + payload += uint16BE(nextPacketID) + nextPacketID &+= 1 + payload += mqttString(topic) + payload.append(0x00) // QoS 0 + connection?.send(content: mqttPacket(type: 0x82, payload: payload), + completion: .idempotent) + } + // MARK: - Encoding helpers private func mqttPacket(type: UInt8, payload: Data) -> Data { diff --git a/SulTee/SulTee/MapContentView.swift b/SulTee/SulTee/MapContentView.swift new file mode 100644 index 0000000..c44dc15 --- /dev/null +++ b/SulTee/SulTee/MapContentView.swift @@ -0,0 +1,184 @@ +import SwiftUI +import MapKit + +/// Full-screen map showing user (blue) and robot (orange) positions, +/// a follow-path line between them, and fading breadcrumb trails for both. +struct MapContentView: View { + @EnvironmentObject var sensor: SensorManager + + @State private var position: MapCameraPosition = .automatic + @State private var followUser = true + + var body: some View { + ZStack(alignment: .bottom) { + map + overlay + } + .ignoresSafeArea(edges: .top) + .onChange(of: sensor.userLocation) { _, coord in + if followUser, let coord { + withAnimation(.easeInOut(duration: 0.4)) { + position = .camera(MapCamera( + centerCoordinate: coord, + distance: 400, + heading: 0, + pitch: 0 + )) + } + } + } + } + + // MARK: - Map + + private var map: some View { + Map(position: $position) { + + // ── User breadcrumb trail (fading blue dots) + let userCrumbs = sensor.userBreadcrumbs + ForEach(userCrumbs.indices, id: \.self) { idx in + let opacity = Double(idx + 1) / Double(max(userCrumbs.count, 1)) + Annotation("", coordinate: userCrumbs[idx]) { + Circle() + .fill(.blue.opacity(opacity * 0.6)) + .frame(width: 7, height: 7) + } + .annotationTitles(.hidden) + } + + // ── Robot breadcrumb trail (fading orange dots) + let robotCrumbs = sensor.robotBreadcrumbs + ForEach(robotCrumbs.indices, id: \.self) { idx in + let opacity = Double(idx + 1) / Double(max(robotCrumbs.count, 1)) + Annotation("", coordinate: robotCrumbs[idx]) { + Circle() + .fill(.orange.opacity(opacity * 0.6)) + .frame(width: 7, height: 7) + } + .annotationTitles(.hidden) + } + + // ── Follow path line: robot → user + if let userLoc = sensor.userLocation, + let robotLoc = sensor.robotLocation { + MapPolyline(coordinates: [robotLoc, userLoc]) + .stroke(.yellow, style: StrokeStyle(lineWidth: 2, dash: [6, 4])) + } + + // ── User marker (blue) + 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) + } + } + } + + // ── Robot marker (orange) + if let robotLoc = sensor.robotLocation { + Annotation("Robot", coordinate: robotLoc) { + ZStack { + Circle() + .fill(.orange.opacity(0.25)) + .frame(width: 36, height: 36) + Image(systemName: "car.fill") + .font(.system(size: 16, weight: .bold)) + .foregroundStyle(.white) + .padding(6) + .background(.orange, in: Circle()) + } + } + } + } + .mapStyle(.standard(elevation: .realistic)) + .onMapCameraChange { _ in + // User dragged map → stop auto-follow + followUser = false + } + .overlay(alignment: .topTrailing) { + followButton + } + } + + // MARK: - Re-centre button + + private var followButton: some View { + Button { + followUser = true + if let coord = sensor.userLocation { + withAnimation { + position = .camera(MapCamera( + centerCoordinate: coord, + distance: 400, + heading: 0, + pitch: 0 + )) + } + } + } label: { + Image(systemName: followUser ? "location.fill" : "location") + .padding(10) + .background(.ultraThinMaterial, in: Circle()) + } + .padding([.top, .trailing], 16) + .padding(.top, 44) // below nav bar + } + + // MARK: - Stats overlay + + private var overlay: some View { + HStack(spacing: 20) { + if let dist = sensor.distanceToRobot { + statCell(value: distanceString(dist), + label: "distance", + icon: "arrow.left.and.right") + } + if sensor.robotSpeed > 0.2 { + statCell(value: String(format: "%.1f m/s", sensor.robotSpeed), + label: "robot spd", + icon: "speedometer") + } + if !sensor.isStreaming { + Text("Start Follow-Me to stream") + .font(.caption) + .foregroundStyle(.secondary) + } + } + .padding(.horizontal, 20) + .padding(.vertical, 12) + .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 16)) + .padding(.bottom, 24) + .padding(.horizontal, 16) + } + + private func statCell(value: String, label: String, icon: String) -> some View { + HStack(spacing: 6) { + Image(systemName: icon) + .foregroundStyle(.secondary) + .font(.caption) + VStack(alignment: .leading, spacing: 0) { + Text(value).font(.headline.monospacedDigit()) + Text(label).font(.caption2).foregroundStyle(.secondary) + } + } + } + + private func distanceString(_ metres: Double) -> String { + metres < 1000 + ? "\(Int(metres)) m" + : String(format: "%.1f km", metres / 1000) + } +} + +#Preview { + MapContentView() + .environmentObject(SensorManager()) +} diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 9604f0a..41db57e 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -1,30 +1,47 @@ import Foundation import CoreLocation import CoreMotion +import MapKit import Combine -/// Manages all iPhone sensors and forwards data to the WebSocket client -/// and MQTT broker (topic: saltybot/ios/gps, 1 Hz). +/// Manages all iPhone sensors, publishes iOS GPS to MQTT, subscribes to robot GPS, +/// and exposes state for the map and status views. final class SensorManager: NSObject, ObservableObject { - // MARK: - Published state for UI + // MARK: - Streaming state @Published var isStreaming = false @Published var wsState: WebSocketClient.ConnectionState = .disconnected - @Published var gpsRate: Double = 0 - @Published var imuRate: Double = 0 - @Published var headingRate: Double = 0 - @Published var baroRate: Double = 0 - @Published var botDistanceMeters: Double? = nil - // MARK: - WebSocket + // MARK: - Sensor rates (Hz) + + @Published var gpsRate: Double = 0 + @Published var imuRate: Double = 0 + @Published var headingRate: Double = 0 + @Published var baroRate: Double = 0 + + // MARK: - User (phone) position + + @Published var userLocation: CLLocationCoordinate2D? = nil + @Published var userBreadcrumbs: [CLLocationCoordinate2D] = [] + + // MARK: - Robot position (from MQTT 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) static let defaultOrinURL = "ws://100.64.0.2:9090" private static let orinURLKey = "orinURL" private(set) var ws: WebSocketClient - /// Current Orin WebSocket URL string (persisted in UserDefaults). var orinURLString: String { get { UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL } set { UserDefaults.standard.set(newValue, forKey: Self.orinURLKey) } @@ -33,29 +50,32 @@ final class SensorManager: NSObject, ObservableObject { // MARK: - MQTT private let mqtt = MQTTClient(config: .init( - host: "192.168.87.29", - port: 1883, + host: "192.168.87.29", + port: 1883, username: "mqtt_seb", password: "mqtt_pass", - clientID: "sultee-ios-\(UUID().uuidString.prefix(8))" + clientID: "saul-t-mote-\(UUID().uuidString.prefix(8))" )) - private static let mqttGPSTopic = "saltybot/ios/gps" + private static let iosGPSTopic = "saltybot/ios/gps" + private static let robotGPSTopic = "saltybot/phone/gps" + private static let maxBreadcrumbs = 60 + private var lastKnownLocation: CLLocation? private var mqttPublishTimer: Timer? // MARK: - Sensors private let locationManager = CLLocationManager() - private let motionManager = CMMotionManager() - private let altimeter = CMAltimeter() - private var cancellables = Set() + private let motionManager = CMMotionManager() + private let altimeter = CMAltimeter() + private var cancellables = Set() - // MARK: - Rate counters (counts per second) + // MARK: - Rate counters - private var gpsCounts: [Date] = [] - private var imuCounts: [Date] = [] + private var gpsCounts: [Date] = [] + private var imuCounts: [Date] = [] private var headingCounts: [Date] = [] - private var baroCounts: [Date] = [] + private var baroCounts: [Date] = [] private var rateTimer: Timer? // MARK: - Init @@ -64,9 +84,9 @@ final class SensorManager: NSObject, ObservableObject { let urlString = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL self.ws = WebSocketClient(url: URL(string: urlString) ?? URL(string: Self.defaultOrinURL)!) super.init() + locationManager.delegate = self locationManager.desiredAccuracy = kCLLocationAccuracyBestForNavigation - // Use dual-frequency GPS (L1+L5) on iPhone 15 Pro — automatic when accuracy is set to Best locationManager.distanceFilter = kCLDistanceFilterNone locationManager.allowsBackgroundLocationUpdates = true locationManager.pausesLocationUpdatesAutomatically = false @@ -76,6 +96,11 @@ final class SensorManager: NSObject, ObservableObject { .receive(on: DispatchQueue.main) .assign(to: \.wsState, on: self) .store(in: &cancellables) + + mqtt.onMessage = { [weak self] topic, payload in + guard let self, topic == Self.robotGPSTopic else { return } + self.handleRobotGPS(payload) + } } // MARK: - Public control @@ -85,6 +110,7 @@ final class SensorManager: NSObject, ObservableObject { isStreaming = true ws.connect() mqtt.connect() + mqtt.subscribe(topic: Self.robotGPSTopic) requestPermissionsAndStartSensors() startRateTimer() startMQTTPublishTimer() @@ -96,14 +122,10 @@ final class SensorManager: NSObject, ObservableObject { ws.disconnect() mqtt.disconnect() stopSensors() - rateTimer?.invalidate() - rateTimer = nil - mqttPublishTimer?.invalidate() - mqttPublishTimer = nil + rateTimer?.invalidate(); rateTimer = nil + mqttPublishTimer?.invalidate(); mqttPublishTimer = nil } - /// Call when the user edits the Orin URL. Persists the value and updates - /// the client URL; takes effect on the next connect(). func updateURL(_ urlString: String) { guard !isStreaming else { return } orinURLString = urlString @@ -134,7 +156,37 @@ final class SensorManager: NSObject, ObservableObject { ] guard let data = try? JSONSerialization.data(withJSONObject: payload), let json = String(data: data, encoding: .utf8) else { return } - mqtt.publish(topic: Self.mqttGPSTopic, payload: json) + mqtt.publish(topic: Self.iosGPSTopic, payload: json) + } + + // MARK: - Robot GPS subscription handler + + 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 + appendBreadcrumb(coord, to: &robotBreadcrumbs) + updateDistance() + } + + // MARK: - Breadcrumbs + distance + + private func appendBreadcrumb(_ coord: CLLocationCoordinate2D, + to list: inout [CLLocationCoordinate2D]) { + list.append(coord) + if list.count > Self.maxBreadcrumbs { list.removeFirst() } + } + + private func updateDistance() { + 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) } // MARK: - Sensor start / stop @@ -157,34 +209,17 @@ final class SensorManager: NSObject, ObservableObject { private func startIMU() { guard motionManager.isDeviceMotionAvailable else { return } motionManager.deviceMotionUpdateInterval = 1.0 / 100.0 // 100 Hz - motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, error in + motionManager.startDeviceMotionUpdates(to: .main) { [weak self] motion, _ in guard let self, let motion else { return } self.recordEvent(in: &self.imuCounts) - let ts = Date().timeIntervalSince1970 self.ws.send([ "type": "imu", - "timestamp": ts, + "timestamp": Date().timeIntervalSince1970, "data": [ - "accel": [ - "x": motion.userAcceleration.x, - "y": motion.userAcceleration.y, - "z": motion.userAcceleration.z - ], - "gyro": [ - "x": motion.rotationRate.x, - "y": motion.rotationRate.y, - "z": motion.rotationRate.z - ], - "attitude": [ - "roll": motion.attitude.roll, - "pitch": motion.attitude.pitch, - "yaw": motion.attitude.yaw - ], - "gravity": [ - "x": motion.gravity.x, - "y": motion.gravity.y, - "z": motion.gravity.z - ], + "accel": ["x": motion.userAcceleration.x, "y": motion.userAcceleration.y, "z": motion.userAcceleration.z], + "gyro": ["x": motion.rotationRate.x, "y": motion.rotationRate.y, "z": motion.rotationRate.z], + "attitude": ["roll": motion.attitude.roll, "pitch": motion.attitude.pitch, "yaw": motion.attitude.yaw], + "gravity": ["x": motion.gravity.x, "y": motion.gravity.y, "z": motion.gravity.z], "magneticField": [ "x": motion.magneticField.field.x, "y": motion.magneticField.field.y, @@ -198,16 +233,14 @@ final class SensorManager: NSObject, ObservableObject { private func startBarometer() { guard CMAltimeter.isRelativeAltitudeAvailable() else { return } - altimeter.startRelativeAltitudeUpdates(to: .main) { [weak self] data, error in + altimeter.startRelativeAltitudeUpdates(to: .main) { [weak self] data, _ in guard let self, let data else { return } self.recordEvent(in: &self.baroCounts) self.ws.send([ "type": "baro", "timestamp": Date().timeIntervalSince1970, - "data": [ - "relativeAltitude": data.relativeAltitude.doubleValue, - "pressure": data.pressure.doubleValue - ] + "data": ["relativeAltitude": data.relativeAltitude.doubleValue, + "pressure": data.pressure.doubleValue] ]) } } @@ -220,22 +253,19 @@ final class SensorManager: NSObject, ObservableObject { } } - private func recordEvent(in list: inout [Date]) { - list.append(Date()) - } + private func recordEvent(in list: inout [Date]) { list.append(Date()) } private func updateRates() { let cutoff = Date().addingTimeInterval(-1.0) - gpsCounts = gpsCounts.filter { $0 > cutoff } - imuCounts = imuCounts.filter { $0 > cutoff } + gpsCounts = gpsCounts.filter { $0 > cutoff } + imuCounts = imuCounts.filter { $0 > cutoff } headingCounts = headingCounts.filter { $0 > cutoff } - baroCounts = baroCounts.filter { $0 > cutoff } - + baroCounts = baroCounts.filter { $0 > cutoff } DispatchQueue.main.async { - self.gpsRate = Double(self.gpsCounts.count) - self.imuRate = Double(self.imuCounts.count) + self.gpsRate = Double(self.gpsCounts.count) + self.imuRate = Double(self.imuCounts.count) self.headingRate = Double(self.headingCounts.count) - self.baroRate = Double(self.baroCounts.count) + self.baroRate = Double(self.baroCounts.count) } } } @@ -247,20 +277,25 @@ 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) + ws.send([ "type": "gps", "timestamp": loc.timestamp.timeIntervalSince1970, "data": [ - "latitude": loc.coordinate.latitude, - "longitude": loc.coordinate.longitude, - "altitude": loc.altitude, - "horizontalAccuracy": loc.horizontalAccuracy, - "verticalAccuracy": loc.verticalAccuracy, - "speed": loc.speed, - "speedAccuracy": loc.speedAccuracy, - "course": loc.course, - "courseAccuracy": loc.courseAccuracy + "latitude": loc.coordinate.latitude, + "longitude": loc.coordinate.longitude, + "altitude": loc.altitude, + "horizontalAccuracy":loc.horizontalAccuracy, + "verticalAccuracy": loc.verticalAccuracy, + "speed": loc.speed, + "speedAccuracy": loc.speedAccuracy, + "course": loc.course, + "courseAccuracy": loc.courseAccuracy ] ]) } @@ -272,11 +307,9 @@ extension SensorManager: CLLocationManagerDelegate { "timestamp": Date().timeIntervalSince1970, "data": [ "magneticHeading": newHeading.magneticHeading, - "trueHeading": newHeading.trueHeading, + "trueHeading": newHeading.trueHeading, "headingAccuracy": newHeading.headingAccuracy, - "x": newHeading.x, - "y": newHeading.y, - "z": newHeading.z + "x": newHeading.x, "y": newHeading.y, "z": newHeading.z ] ]) } @@ -292,8 +325,7 @@ extension SensorManager: CLLocationManagerDelegate { manager.startUpdatingLocation() manager.startUpdatingHeading() } - default: - break + default: break } } } -- 2.47.2 From 1d5f196e6843756e95a7f03ab86b4d76bea50a3a Mon Sep 17 00:00:00 2001 From: sl-ios Date: Sat, 4 Apr 2026 12:12:21 -0400 Subject: [PATCH 06/19] =?UTF-8?q?feat:=20Add=20UWB=20integration=20?= =?UTF-8?q?=E2=80=94=20follow=20mode,=20range=20presets,=20UWB=20status?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- SulTee/SulTee.xcodeproj/project.pbxproj | 4 + SulTee/SulTee/MapContentView.swift | 203 ++++++++++++++++++------ SulTee/SulTee/SensorManager.swift | 147 +++++++++++++---- SulTee/SulTee/UWBModels.swift | 51 ++++++ 4 files changed, 325 insertions(+), 80 deletions(-) create mode 100644 SulTee/SulTee/UWBModels.swift diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index 1a6ff66..a65c916 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -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 = ""; }; A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = ""; }; + A10000010000000000000BAB /* UWBModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = UWBModels.swift; sourceTree = ""; }; /* 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; }; diff --git a/SulTee/SulTee/MapContentView.swift b/SulTee/SulTee/MapContentView.swift index c44dc15..d317324 100644 --- a/SulTee/SulTee/MapContentView.swift +++ b/SulTee/SulTee/MapContentView.swift @@ -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 + @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 @@ -59,7 +86,7 @@ struct MapContentView: View { } // ── Follow path line: robot → user - if let userLoc = sensor.userLocation, + if let userLoc = sensor.userLocation, let robotLoc = sensor.robotLocation { MapPolyline(coordinates: [robotLoc, userLoc]) .stroke(.yellow, style: StrokeStyle(lineWidth: 2, dash: [6, 4])) @@ -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), - label: "distance", - icon: "arrow.left.and.right") + statCell( + value: distanceString(dist), + label: "distance", + 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) } } diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 41db57e..34a9121 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -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,19 +25,27 @@ 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 + @Published var distanceToRobot: Double? = nil - // MARK: - Derived + // MARK: - UWB (saltybot/uwb/range + saltybot/uwb/position) - @Published var distanceToRobot: Double? = nil + @Published var uwbPosition: UWBPosition? = nil + @Published var uwbRanges: [String: UWBRange] = [:] // anchorID → UWBRange + @Published var uwbActive: Bool = false - // MARK: - WebSocket config (sensor stream to Orin) + // MARK: - Follow settings - static let defaultOrinURL = "ws://100.64.0.2:9090" + @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" private(set) var ws: WebSocketClient @@ -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 maxBreadcrumbs = 60 - private var lastKnownLocation: CLLocation? - private var mqttPublishTimer: Timer? + 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 @@ -76,13 +91,13 @@ final class SensorManager: NSObject, ObservableObject { private var imuCounts: [Date] = [] private var headingCounts: [Date] = [] private var baroCounts: [Date] = [] - private var rateTimer: Timer? + private var rateTimer: Timer? // 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() { @@ -122,8 +148,9 @@ final class SensorManager: NSObject, ObservableObject { ws.disconnect() mqtt.disconnect() stopSensors() - rateTimer?.invalidate(); rateTimer = nil - mqttPublishTimer?.invalidate(); mqttPublishTimer = nil + 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 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) @@ -287,15 +368,15 @@ extension SensorManager: CLLocationManagerDelegate { "type": "gps", "timestamp": loc.timestamp.timeIntervalSince1970, "data": [ - "latitude": loc.coordinate.latitude, - "longitude": loc.coordinate.longitude, - "altitude": loc.altitude, - "horizontalAccuracy":loc.horizontalAccuracy, - "verticalAccuracy": loc.verticalAccuracy, - "speed": loc.speed, - "speedAccuracy": loc.speedAccuracy, - "course": loc.course, - "courseAccuracy": loc.courseAccuracy + "latitude": loc.coordinate.latitude, + "longitude": loc.coordinate.longitude, + "altitude": loc.altitude, + "horizontalAccuracy": loc.horizontalAccuracy, + "verticalAccuracy": loc.verticalAccuracy, + "speed": loc.speed, + "speedAccuracy": loc.speedAccuracy, + "course": loc.course, + "courseAccuracy": loc.courseAccuracy ] ]) } diff --git a/SulTee/SulTee/UWBModels.swift b/SulTee/SulTee/UWBModels.swift new file mode 100644 index 0000000..90efff3 --- /dev/null +++ b/SulTee/SulTee/UWBModels.swift @@ -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 } +} -- 2.47.2 From c472668d7a515953ec4457138102c1b29c757717 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Sat, 4 Apr 2026 12:22:17 -0400 Subject: [PATCH 07/19] =?UTF-8?q?feat:=20Merge=20SaltyTag=20BLE=20?= =?UTF-8?q?=E2=80=94=20GPS/IMU=20streaming=20to=20UWB=20tag,=20anchor=20di?= =?UTF-8?q?splay,=20UWB=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() } -- 2.47.2 From 7b911d3591cf46baf2afc380c6ec21950c38047d Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 15:20:30 -0400 Subject: [PATCH 08/19] fix: scan all peripherals so NimBLE service UUID in scan response is not missed iOS CoreBluetooth only fires didDiscover when the service UUID is in the primary ADV_IND packet. NimBLE (ESP32) puts service UUIDs in the scan response by default, so scanForPeripherals(withServices:[uuid]) never returned the UWB_TAG device. Fix: scan with withServices:nil and filter by device name prefix "UWB_TAG" in didDiscover. Also fix the broken OR guard (|| name.isEmpty == false passed any named peripheral) to a clean hasPrefix check. Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/BLEManager.swift | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/SulTee/SulTee/BLEManager.swift b/SulTee/SulTee/BLEManager.swift index 376cd7c..d468576 100644 --- a/SulTee/SulTee/BLEManager.swift +++ b/SulTee/SulTee/BLEManager.swift @@ -82,7 +82,11 @@ final class BLEManager: NSObject, ObservableObject { private func doStartScan() { DispatchQueue.main.async { self.connectionState = .scanning } - central.scanForPeripherals(withServices: [Self.serviceUUID], + // Scan for ALL peripherals — NimBLE (ESP32) puts service UUIDs in the scan + // response, not the primary advertisement. iOS won't call didDiscover for + // service-filtered scans unless the UUID is in the primary ADV_IND packet. + // We filter by device name prefix in didDiscover instead. + central.scanForPeripherals(withServices: nil, options: [CBCentralManagerScanOptionAllowDuplicatesKey: false]) // Auto-stop after 15 s if nothing found scanTimer?.invalidate() @@ -122,9 +126,8 @@ extension BLEManager: CBCentralManagerDelegate { 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 } + // Only connect to UWB_TAG devices (name prefix set by NimBLE on tag firmware) + guard let name = peripheral.name, name.hasPrefix("UWB_TAG") else { return } stopScan() self.peripheral = peripheral peripheral.delegate = self -- 2.47.2 From 7f9f159016ad24ac9d1c9a611aa82dc531e92448 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 15:48:58 -0400 Subject: [PATCH 09/19] fix: resolve all Swift compiler errors and BLE write type mismatch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - BLEManager: fall back to withResponse write when characteristic lacks PROPERTY_WRITE_NR (0x4); NimBLE defaults to PROPERTY_WRITE (0x8) only, causing iOS to silently drop every withoutResponse write at 5/10 Hz - BLEManager: scan withServices:nil so NimBLE scan-response UUIDs are found; filter by UWB_TAG name prefix in didDiscover - BLEPackets: remove custom clamping extensions (Int16/Int32/UInt16/UInt8) that shadowed Swift.max() with Int16.max inside the extension scope; stdlib BinaryInteger.init(clamping:) covers all cases - BLEStatusView: use explicit Binding(get:set:) for gpsStreamEnabled / imuStreamEnabled — SwiftUI cannot synthesize $binding through a let computed property backed by a class reference - SensorManager: fix isRelativeAltitudeAvailable() — it is a class method, not an instance method; also fixed inverted double-negative logic Note for HAL: add NimBLE PROPERTY_WRITE_NR to GPS (abcdef3) and IMU (abcdef4) characteristics for no-ACK streaming at 5/10 Hz. Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/BLEManager.swift | 11 +++++++++-- SulTee/SulTee/BLEPackets.swift | 22 ---------------------- SulTee/SulTee/BLEStatusView.swift | 4 ++-- SulTee/SulTee/SensorManager.swift | 2 +- 4 files changed, 12 insertions(+), 27 deletions(-) diff --git a/SulTee/SulTee/BLEManager.swift b/SulTee/SulTee/BLEManager.swift index d468576..5dd3906 100644 --- a/SulTee/SulTee/BLEManager.swift +++ b/SulTee/SulTee/BLEManager.swift @@ -68,14 +68,21 @@ final class BLEManager: NSObject, ObservableObject { func sendGPS(_ data: Data) { guard gpsStreamEnabled, isConnected, let p = peripheral, let c = gpsChar else { return } - p.writeValue(data, for: c, type: .withoutResponse) + // Use writeWithoutResponse if the characteristic supports it (lower latency, + // no ACK overhead). Fall back to write-with-response if the firmware only + // advertises PROPERTY_WRITE (0x08) — e.g. NimBLE default without WRITE_NR. + let writeType: CBCharacteristicWriteType = + c.properties.contains(.writeWithoutResponse) ? .withoutResponse : .withResponse + p.writeValue(data, for: c, type: writeType) } /// 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) + let writeType: CBCharacteristicWriteType = + c.properties.contains(.writeWithoutResponse) ? .withoutResponse : .withResponse + p.writeValue(data, for: c, type: writeType) } // MARK: - Internal helpers diff --git a/SulTee/SulTee/BLEPackets.swift b/SulTee/SulTee/BLEPackets.swift index fb655bb..f13adeb 100644 --- a/SulTee/SulTee/BLEPackets.swift +++ b/SulTee/SulTee/BLEPackets.swift @@ -164,25 +164,3 @@ private extension Data { } } -// 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 index 2f4d559..41a4618 100644 --- a/SulTee/SulTee/BLEStatusView.swift +++ b/SulTee/SulTee/BLEStatusView.swift @@ -70,7 +70,7 @@ struct BLEStatusView: View { private var streamingSection: some View { Section("Data Streaming") { - Toggle(isOn: $sensor.ble.gpsStreamEnabled) { + Toggle(isOn: Binding(get: { ble.gpsStreamEnabled }, set: { ble.gpsStreamEnabled = $0 })) { Label { VStack(alignment: .leading, spacing: 2) { Text("GPS → Tag") @@ -82,7 +82,7 @@ struct BLEStatusView: View { .foregroundStyle(.blue) } } - Toggle(isOn: $sensor.ble.imuStreamEnabled) { + Toggle(isOn: Binding(get: { ble.imuStreamEnabled }, set: { ble.imuStreamEnabled = $0 })) { Label { VStack(alignment: .leading, spacing: 2) { Text("IMU → Tag") diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 8ed014e..c8e8a27 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -229,7 +229,7 @@ final class SensorManager: NSObject, ObservableObject { locationManager.startUpdatingLocation() locationManager.startUpdatingHeading() if !motionManager.isDeviceMotionActive { startIMU() } - if !altimeter.isRelativeAltitudeAvailable() == false { startBarometer() } + if CMAltimeter.isRelativeAltitudeAvailable() { startBarometer() } } private func stopSensors() { -- 2.47.2 From 12338f491eff864408e51248f8e8f75e5719eb14 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 16:25:35 -0400 Subject: [PATCH 10/19] =?UTF-8?q?fix:=20parse=20HAL=20two-anchor=20ranging?= =?UTF-8?q?=20format=20(8-byte=20int32=C3=972)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The app's old parser expected a multi-anchor protocol: [count][anchorID+rangeMM+RSSI+age]×N (min 10 bytes) HAL's firmware sends a fixed 8-byte packet: [int32 front_mm LE][int32 back_mm LE] With the old parser data[0] was interpreted as anchor count (e.g. 0xF8 = 248 for a 4.6m reading), the loop guard failed immediately, and every notify returned [] — hence "waiting for ranging data" despite the tag showing live ranges on OLED. Changes: - BLEPackets: detect 8-byte HAL format by length; decode as anchor id=0 (Front) and id=1 (Back); legacy multi-anchor path retained for forward compatibility - AnchorInfo: rssiDBm is now Optional (nil when not reported); label maps id 0→"F", 1→"B" for the two-anchor HAL format - BLEStatusView: guard on optional rssiString before rendering Auto-reconnect confirmed correct (2s delay, bluetooth-central background mode declared in Info.plist). Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/AnchorInfo.swift | 20 ++++++++++++---- SulTee/SulTee/BLEPackets.swift | 40 +++++++++++++++++++++++-------- SulTee/SulTee/BLEStatusView.swift | 8 ++++--- 3 files changed, 50 insertions(+), 18 deletions(-) diff --git a/SulTee/SulTee/AnchorInfo.swift b/SulTee/SulTee/AnchorInfo.swift index fef8c36..fa08bcd 100644 --- a/SulTee/SulTee/AnchorInfo.swift +++ b/SulTee/SulTee/AnchorInfo.swift @@ -4,15 +4,22 @@ import Foundation struct AnchorInfo: Identifiable { let id: UInt8 let rangeMetres: Double - let rssiDBm: Double - let ageMs: UInt16 // age reported by tag firmware + let rssiDBm: Double? // nil when not reported (HAL two-anchor format) + let ageMs: UInt16 // age reported by tag firmware; 0 when not reported 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)" } + /// IDs 0/1 map to Front/Back (HAL two-anchor format); others show "A". + var label: String { + switch id { + case 0: return "F" + case 1: return "B" + default: return "A\(id)" + } + } /// Formatted range string matching the Flutter app style. var rangeString: String { @@ -21,6 +28,9 @@ struct AnchorInfo: Identifiable { : String(format: "%.1f m", rangeMetres) } - /// Formatted RSSI string. - var rssiString: String { "\(Int(rssiDBm.rounded())) dBm" } + /// Formatted RSSI string, or nil when RSSI was not reported. + var rssiString: String? { + guard let r = rssiDBm else { return nil } + return "\(Int(r.rounded())) dBm" + } } diff --git a/SulTee/SulTee/BLEPackets.swift b/SulTee/SulTee/BLEPackets.swift index f13adeb..e92de49 100644 --- a/SulTee/SulTee/BLEPackets.swift +++ b/SulTee/SulTee/BLEPackets.swift @@ -87,27 +87,46 @@ enum BLEPackets { // 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 + // HAL firmware format (8 bytes, 2 fixed anchors): + // [0-3] Int32 LE front anchor range mm (id=0) + // [4-7] Int32 LE back anchor range mm (id=1) + // + // Legacy multi-anchor format (future): + // [0] Uint8 anchor count N + // Per anchor (9 bytes): + // [+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] { + let now = Date() + + // HAL two-anchor format: exactly 8 bytes, two Int32 LE range values + if data.count == 8 { + let frontMM = data.readInt32LE(at: 0) + let backMM = data.readInt32LE(at: 4) + return [ + AnchorInfo(id: 0, rangeMetres: Double(frontMM) / 1000.0, + rssiDBm: nil, ageMs: 0, receivedAt: now), + AnchorInfo(id: 1, rangeMetres: Double(backMM) / 1000.0, + rssiDBm: nil, ageMs: 0, receivedAt: now) + ] + } + + // Legacy multi-anchor format: [count][anchorID+rangeMM+RSSI+age] × N guard data.count >= 1 else { return [] } let count = Int(data[0]) - let now = Date() var result: [AnchorInfo] = [] for i in 0.. Date: Mon, 6 Apr 2026 16:29:38 -0400 Subject: [PATCH 11/19] fix: support v3.4 12-byte ranging packet with best_rssi float MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit HAL v3.4 extends the range notify from 8→12 bytes: [0-3] int32 LE front_mm [4-7] int32 LE back_mm [8-11] float32 LE best_rssi dBm (NEW) Parser now handles both lengths (8=v3.3, 12=v3.4) by detecting packet size. RSSI is extracted as Double and assigned to both Front and Back anchors (shared signal quality); nil for v3.3. UI already conditionally renders rssiString (guard on Optional). Added Data.readFloat32LE(at:) helper using IEEE 754 bit-pattern reinterpretation (little-endian UInt32 → Float(bitPattern:)). Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/BLEPackets.swift | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/SulTee/SulTee/BLEPackets.swift b/SulTee/SulTee/BLEPackets.swift index e92de49..592a549 100644 --- a/SulTee/SulTee/BLEPackets.swift +++ b/SulTee/SulTee/BLEPackets.swift @@ -87,9 +87,14 @@ enum BLEPackets { // MARK: - Ranging notification parser // - // HAL firmware format (8 bytes, 2 fixed anchors): - // [0-3] Int32 LE front anchor range mm (id=0) - // [4-7] Int32 LE back anchor range mm (id=1) + // HAL firmware format v3.3 (8 bytes, 2 fixed anchors): + // [0-3] Int32 LE front anchor range mm (id=0) + // [4-7] Int32 LE back anchor range mm (id=1) + // + // HAL firmware format v3.4 (12 bytes, adds signal quality): + // [0-3] Int32 LE front anchor range mm (id=0) + // [4-7] Int32 LE back anchor range mm (id=1) + // [8-11] Float32 LE best_rssi dBm (shared; assigned to both anchors) // // Legacy multi-anchor format (future): // [0] Uint8 anchor count N @@ -102,15 +107,18 @@ enum BLEPackets { static func parseRanging(_ data: Data) -> [AnchorInfo] { let now = Date() - // HAL two-anchor format: exactly 8 bytes, two Int32 LE range values - if data.count == 8 { + // HAL two-anchor format: 8 bytes (v3.3) or 12 bytes (v3.4 + RSSI float) + if data.count == 8 || data.count == 12 { let frontMM = data.readInt32LE(at: 0) let backMM = data.readInt32LE(at: 4) + let rssi: Double? = data.count == 12 + ? Double(data.readFloat32LE(at: 8)) + : nil return [ AnchorInfo(id: 0, rangeMetres: Double(frontMM) / 1000.0, - rssiDBm: nil, ageMs: 0, receivedAt: now), + rssiDBm: rssi, ageMs: 0, receivedAt: now), AnchorInfo(id: 1, rangeMetres: Double(backMM) / 1000.0, - rssiDBm: nil, ageMs: 0, receivedAt: now) + rssiDBm: rssi, ageMs: 0, receivedAt: now) ] } @@ -182,5 +190,12 @@ private extension Data { func readUInt16LE(at offset: Int) -> UInt16 { UInt16(self[offset]) | (UInt16(self[offset + 1]) << 8) } + func readFloat32LE(at offset: Int) -> Float { + let bits = UInt32(self[offset]) + | (UInt32(self[offset + 1]) << 8) + | (UInt32(self[offset + 2]) << 16) + | (UInt32(self[offset + 3]) << 24) + return Float(bitPattern: bits) + } } -- 2.47.2 From efeff9e6c0a1d1ea0dc3d0880a5b1f2663006672 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 16:35:11 -0400 Subject: [PATCH 12/19] config: change default WebSocket URL to wss://www.saultee.bot/ws Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/SensorManager.swift | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index c8e8a27..20481f3 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -68,7 +68,7 @@ final class SensorManager: NSObject, ObservableObject { // MARK: - WebSocket config - static let defaultOrinURL = "ws://100.64.0.2:9090" + static let defaultOrinURL = "wss://www.saultee.bot/ws" private static let orinURLKey = "orinURL" private(set) var ws: WebSocketClient -- 2.47.2 From 313e84a5166295c449cdffd54b1d5146f7ff1e2e Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 16:44:43 -0400 Subject: [PATCH 13/19] fix: migrate stale UserDefaults WebSocket URL on launch UserDefaults persists across app reinstalls. Any device that previously stored ws://100.64.0.2:9090 would ignore the new defaultOrinURL constant. On init, if the saved URL contains '100.64.0.2' it is cleared so the new default wss://www.saultee.bot/ws is used on next launch. Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/SensorManager.swift | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 20481f3..af9f6ea 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -132,6 +132,11 @@ final class SensorManager: NSObject, ObservableObject { // MARK: - Init override init() { + // Migrate: if the stored URL is the old Tailscale IP, replace with the new WSS endpoint. + if let saved = UserDefaults.standard.string(forKey: Self.orinURLKey), + saved.contains("100.64.0.2") { + UserDefaults.standard.removeObject(forKey: Self.orinURLKey) + } let urlStr = UserDefaults.standard.string(forKey: Self.orinURLKey) ?? Self.defaultOrinURL self.ws = WebSocketClient(url: URL(string: urlStr) ?? URL(string: Self.defaultOrinURL)!) super.init() -- 2.47.2 From b2bda0f467f81af5c71caf8f0cecba3d075624e3 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 17:24:39 -0400 Subject: [PATCH 14/19] 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 -- 2.47.2 From 6fa2a1b03fa0b190bfeaaa1caaeec377dce1d0f8 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 17:31:30 -0400 Subject: [PATCH 15/19] =?UTF-8?q?fix:=20throttle=20BLE=20writes=20to=20TAG?= =?UTF-8?q?=20=E2=80=94=20GPS=201Hz,=20IMU=202Hz?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Tag was crashing ~5s after BLE connect due to GPS_RX at 5Hz + IMU_RX at 10Hz flooding NimBLE while DW1000 UWB SPI also runs. Combined 15 writes/sec exceeded ESP32 processing capacity. GPS: 0.2s interval → 1.0s (5Hz → 1Hz) IMU: 0.1s interval → 0.5s (10Hz → 2Hz) Total BLE write load: 15/s → 3/s Updated BLEStatusView captions to match new rates. Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/BLEStatusView.swift | 4 ++-- SulTee/SulTee/SensorManager.swift | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/SulTee/SulTee/BLEStatusView.swift b/SulTee/SulTee/BLEStatusView.swift index b9548ae..73570f1 100644 --- a/SulTee/SulTee/BLEStatusView.swift +++ b/SulTee/SulTee/BLEStatusView.swift @@ -74,7 +74,7 @@ struct BLEStatusView: View { Label { VStack(alignment: .leading, spacing: 2) { Text("GPS → Tag") - Text("5 Hz · 20 bytes/packet") + Text("1 Hz · 20 bytes/packet") .font(.caption).foregroundStyle(.secondary) } } icon: { @@ -86,7 +86,7 @@ struct BLEStatusView: View { Label { VStack(alignment: .leading, spacing: 2) { Text("IMU → Tag") - Text("10 Hz · 22 bytes/packet (accel + gyro + mag)") + Text("2 Hz · 22 bytes/packet (accel + gyro + mag)") .font(.caption).foregroundStyle(.secondary) } } icon: { diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index c3b3820..32dfc51 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -116,8 +116,8 @@ final class SensorManager: NSObject, ObservableObject { // 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 bleGPSTimer: Timer? // 1 Hz BLE GPS write + private var bleIMUTimer: Timer? // 2 Hz BLE IMU write private var uwbStalenessTimer: Timer? private var rateTimer: Timer? @@ -263,13 +263,13 @@ final class SensorManager: NSObject, ObservableObject { private func startBLETimers() { stopBLETimers() - // GPS → tag at 5 Hz (200 ms) - bleGPSTimer = Timer.scheduledTimer(withTimeInterval: 0.2, repeats: true) { [weak self] _ in + // GPS → tag at 1 Hz (1000 ms) — throttled to avoid flooding constrained ESP32 + bleGPSTimer = Timer.scheduledTimer(withTimeInterval: 1.0, 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 + // IMU → tag at 2 Hz (500 ms) — throttled to avoid flooding constrained ESP32 + bleIMUTimer = Timer.scheduledTimer(withTimeInterval: 0.5, repeats: true) { [weak self] _ in guard let self, let motion = self.lastKnownMotion else { return } self.ble.sendIMU(BLEPackets.imuPacket(from: motion)) } -- 2.47.2 From cd90d6dbeed4cd9b8e63ee1b759a2064c995afe7 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 18:58:33 -0400 Subject: [PATCH 16/19] =?UTF-8?q?feat:=20Phase=201=20=E2=80=94=20Route=20R?= =?UTF-8?q?ecording=20with=20waypoints=20(GPS=20track=20at=201Hz)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit New 'Routes' tab added to SAUL-T-MOTE: RECORDING - Record button starts 1Hz GPS capture (lat/lon/alt/speed/bearing/ts) - Live stats bar: elapsed time, point count, distance, waypoint count - Live map shows recorded polyline + waypoint annotations in real-time - 'Add Waypoint' sheet: label + robot action (none/stop/slow/photo) - 'Stop' ends recording → Save sheet to name the route STORAGE - JSON files in app Documents/routes/.json - RouteStore: save/rename/delete; auto-sorts newest first - Route list with duration, distance, waypoint count MQTT FORMAT DEFINED (Phase 3 playback — robot side TBD) - Topic: saltybot/route/command - Payload: {action, route_id, route_name, points:[{lat,lon,alt,speed,bearing,ts}], waypoints:[{lat,lon,alt,ts,label,action}]} New files: RouteModels.swift, RouteStore.swift, RouteRecorder.swift, RoutesView.swift SensorManager: lastKnownLocation promoted to private(set) for recorder access Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee.xcodeproj/project.pbxproj | 16 ++ SulTee/SulTee/ContentView.swift | 2 + SulTee/SulTee/RouteModels.swift | 117 +++++++++ SulTee/SulTee/RouteRecorder.swift | 112 +++++++++ SulTee/SulTee/RouteStore.swift | 63 +++++ SulTee/SulTee/RoutesView.swift | 304 ++++++++++++++++++++++++ SulTee/SulTee/SensorManager.swift | 2 +- 7 files changed, 615 insertions(+), 1 deletion(-) create mode 100644 SulTee/SulTee/RouteModels.swift create mode 100644 SulTee/SulTee/RouteRecorder.swift create mode 100644 SulTee/SulTee/RouteStore.swift create mode 100644 SulTee/SulTee/RoutesView.swift diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index 969f5f3..67a3785 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -16,6 +16,10 @@ 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 */; }; + A100000100000000000010AB /* RouteModels.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000010BB /* RouteModels.swift */; }; + A100000100000000000011AB /* RouteStore.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000011BB /* RouteStore.swift */; }; + A100000100000000000012AB /* RouteRecorder.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000012BB /* RouteRecorder.swift */; }; + A100000100000000000013AB /* RoutesView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000013BB /* RoutesView.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 */; }; @@ -33,6 +37,10 @@ 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 = ""; }; + A100000100000000000010BB /* RouteModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RouteModels.swift; sourceTree = ""; }; + A100000100000000000011BB /* RouteStore.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RouteStore.swift; sourceTree = ""; }; + A100000100000000000012BB /* RouteRecorder.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RouteRecorder.swift; sourceTree = ""; }; + A100000100000000000013BB /* RoutesView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RoutesView.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 = ""; }; @@ -71,6 +79,10 @@ A10000010000000000000DAB /* BLEPackets.swift */, A10000010000000000000EAB /* AnchorInfo.swift */, A10000010000000000000FAB /* BLEStatusView.swift */, + A100000100000000000010BB /* RouteModels.swift */, + A100000100000000000011BB /* RouteStore.swift */, + A100000100000000000012BB /* RouteRecorder.swift */, + A100000100000000000013BB /* RoutesView.swift */, A100000100000000000005AB /* Assets.xcassets */, A100000100000000000006AB /* Info.plist */, ); @@ -166,6 +178,10 @@ A10000010000000000000DAA /* BLEPackets.swift in Sources */, A10000010000000000000EAA /* AnchorInfo.swift in Sources */, A10000010000000000000FAA /* BLEStatusView.swift in Sources */, + A100000100000000000010AB /* RouteModels.swift in Sources */, + A100000100000000000011AB /* RouteStore.swift in Sources */, + A100000100000000000012AB /* RouteRecorder.swift in Sources */, + A100000100000000000013AB /* RoutesView.swift in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift index 5443342..35f3ba1 100644 --- a/SulTee/SulTee/ContentView.swift +++ b/SulTee/SulTee/ContentView.swift @@ -13,6 +13,8 @@ struct ContentView: View { .tabItem { Label("Map", systemImage: "map.fill") } BLEStatusView() .tabItem { Label("BLE Tag", systemImage: "dot.radiowaves.right") } + RoutesView() + .tabItem { Label("Routes", systemImage: "point.bottomleft.forward.to.point.topright.scurvepath") } } } } diff --git a/SulTee/SulTee/RouteModels.swift b/SulTee/SulTee/RouteModels.swift new file mode 100644 index 0000000..7931ea5 --- /dev/null +++ b/SulTee/SulTee/RouteModels.swift @@ -0,0 +1,117 @@ +import Foundation +import CoreLocation + +// MARK: - Route data types + +/// A single GPS sample recorded at 1 Hz. +struct RoutePoint: Codable { + let latitude: Double + let longitude: Double + let altitude: Double + let speed: Double // m/s + let bearing: Double // degrees, 0–360 + let timestamp: Double // Unix epoch seconds +} + +/// A named marker added by the user during recording. +struct Waypoint: Codable, Identifiable { + let id: UUID + let latitude: Double + let longitude: Double + let altitude: Double + let timestamp: Double + var label: String + var action: WaypointAction + + init(location: CLLocation, label: String, action: WaypointAction = .none) { + self.id = UUID() + self.latitude = location.coordinate.latitude + self.longitude = location.coordinate.longitude + self.altitude = location.altitude + self.timestamp = location.timestamp.timeIntervalSince1970 + self.label = label + self.action = action + } +} + +enum WaypointAction: String, Codable, CaseIterable, Identifiable { + case none = "none" + case stop = "stop" + case slow = "slow" + case photo = "photo" + + var id: String { rawValue } + + var displayName: String { + switch self { + case .none: return "Marker" + case .stop: return "Stop" + case .slow: return "Slow down" + case .photo: return "Take photo" + } + } + + var systemImage: String { + switch self { + case .none: return "mappin" + case .stop: return "stop.circle" + case .slow: return "tortoise" + case .photo: return "camera" + } + } +} + +/// A complete recorded route persisted to disk. +struct SavedRoute: Codable, Identifiable { + let id: UUID + var name: String + let date: Date + var points: [RoutePoint] + var waypoints: [Waypoint] + + /// Total elapsed recording time in seconds. + var durationSeconds: Double { + guard let first = points.first, let last = points.last else { return 0 } + return last.timestamp - first.timestamp + } + + /// Approximate distance in metres (sum of point-to-point segments). + var distanceMetres: Double { + var total = 0.0 + for i in 1..", + // "route_name":"", + // "points": [{"lat","lon","alt","speed","bearing","ts"}, ...], + // "waypoints": [{"lat","lon","alt","ts","label","action"}, ...] + // } + + func mqttPayload(action: String = "start") -> [String: Any] { + [ + "action": action, + "route_id": id.uuidString, + "route_name": name, + "points": points.map {[ + "lat": $0.latitude, "lon": $0.longitude, + "alt": $0.altitude, "speed": $0.speed, + "bearing": $0.bearing, "ts": $0.timestamp + ]}, + "waypoints": waypoints.map {[ + "lat": $0.latitude, "lon": $0.longitude, + "alt": $0.altitude, "ts": $0.timestamp, + "label": $0.label, "action": $0.action.rawValue + ]} + ] + } +} diff --git a/SulTee/SulTee/RouteRecorder.swift b/SulTee/SulTee/RouteRecorder.swift new file mode 100644 index 0000000..ca93c36 --- /dev/null +++ b/SulTee/SulTee/RouteRecorder.swift @@ -0,0 +1,112 @@ +import Foundation +import CoreLocation +import Combine + +/// Records a GPS track at 1 Hz and collects named waypoints. +/// Observes SensorManager.lastKnownLocation — no duplicate location manager needed. +final class RouteRecorder: ObservableObject { + + enum State { case idle, recording } + + @Published private(set) var state: State = .idle + @Published private(set) var points: [RoutePoint] = [] + @Published private(set) var waypoints: [Waypoint] = [] + @Published private(set) var elapsedSeconds: Double = 0 + + private weak var sensorManager: SensorManager? + private var recordTimer: Timer? + private var elapsedTimer: Timer? + + init(sensorManager: SensorManager) { + self.sensorManager = sensorManager + } + + // MARK: - Public API + + func startRecording() { + guard state == .idle else { return } + points = [] + waypoints = [] + elapsedSeconds = 0 + state = .recording + + // Capture one GPS point immediately, then every 1 s + capturePoint() + recordTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in + self?.capturePoint() + } + elapsedTimer = Timer.scheduledTimer(withTimeInterval: 1.0, repeats: true) { [weak self] _ in + self?.elapsedSeconds += 1 + } + } + + /// Stops recording and returns the finished SavedRoute (not yet persisted). + @discardableResult + func stopRecording(name: String? = nil) -> SavedRoute? { + guard state == .recording else { return nil } + recordTimer?.invalidate(); recordTimer = nil + elapsedTimer?.invalidate(); elapsedTimer = nil + state = .idle + + guard !points.isEmpty else { return nil } + + let routeName = name ?? defaultName() + let route = SavedRoute( + id: UUID(), + name: routeName, + date: Date(), + points: points, + waypoints: waypoints + ) + points = [] + waypoints = [] + return route + } + + /// Adds a waypoint at the current GPS position. + func addWaypoint(label: String, action: WaypointAction = .none) { + guard state == .recording, + let loc = sensorManager?.lastKnownLocation else { return } + let wp = Waypoint(location: loc, label: label, action: action) + waypoints.append(wp) + } + + // MARK: - Private + + private func capturePoint() { + guard let loc = sensorManager?.lastKnownLocation else { return } + let bearing = loc.course >= 0 ? loc.course : 0 + let pt = RoutePoint( + latitude: loc.coordinate.latitude, + longitude: loc.coordinate.longitude, + altitude: loc.altitude, + speed: max(0, loc.speed), + bearing: bearing, + timestamp: loc.timestamp.timeIntervalSince1970 + ) + points.append(pt) + } + + private func defaultName() -> String { + let fmt = DateFormatter() + fmt.dateFormat = "MMM d, HH:mm" + return "Route \(fmt.string(from: Date()))" + } + + // MARK: - Computed helpers for UI + + var distanceSoFar: Double { + var total = 0.0 + for i in 1.. $1.date } + } + } + + func rename(_ route: SavedRoute, to name: String) { + var updated = route + updated.name = name + save(updated) + } + + func delete(_ route: SavedRoute) { + try? FileManager.default.removeItem(at: fileURL(for: route.id)) + routes.removeAll { $0.id == route.id } + } + + // MARK: - Private + + private func load() { + guard let files = try? FileManager.default.contentsOfDirectory( + at: directory, includingPropertiesForKeys: nil + ) else { return } + + routes = files + .filter { $0.pathExtension == "json" } + .compactMap { url -> SavedRoute? in + guard let data = try? Data(contentsOf: url) else { return nil } + return try? JSONDecoder().decode(SavedRoute.self, from: data) + } + .sorted { $0.date > $1.date } + } + + private func fileURL(for id: UUID) -> URL { + directory.appendingPathComponent("\(id.uuidString).json") + } +} diff --git a/SulTee/SulTee/RoutesView.swift b/SulTee/SulTee/RoutesView.swift new file mode 100644 index 0000000..5f5aa50 --- /dev/null +++ b/SulTee/SulTee/RoutesView.swift @@ -0,0 +1,304 @@ +import SwiftUI +import MapKit + +// MARK: - Routes tab root + +struct RoutesView: View { + @EnvironmentObject var sensor: SensorManager + @StateObject private var store = RouteStore() + + // Recorder is created lazily in onAppear (needs sensor reference) + @State private var recorder: RouteRecorder? + + @State private var showWaypointSheet = false + @State private var waypointLabel = "" + @State private var waypointAction = WaypointAction.none + @State private var showSaveSheet = false + @State private var pendingRoute: SavedRoute? + @State private var routeName = "" + + var body: some View { + NavigationStack { + Group { + if let rec = recorder, rec.state == .recording { + recordingView(rec) + } else { + routeListView + } + } + .navigationTitle("Routes") + .toolbar { + if recorder?.state != .recording { + ToolbarItem(placement: .primaryAction) { + Button { + let rec = recorder ?? RouteRecorder(sensorManager: sensor) + recorder = rec + rec.startRecording() + } label: { + Label("Record", systemImage: "record.circle") + .foregroundStyle(.red) + } + } + } + } + } + .onAppear { + if recorder == nil { recorder = RouteRecorder(sensorManager: sensor) } + } + .sheet(isPresented: $showWaypointSheet) { waypointSheet } + .sheet(isPresented: $showSaveSheet) { + if let route = pendingRoute { saveRouteSheet(route) } + } + } + + // MARK: - Route list + + private var routeListView: some View { + List { + if store.routes.isEmpty { + ContentUnavailableView( + "No Routes", + systemImage: "point.bottomleft.forward.to.point.topright.scurvepath", + description: Text("Tap the record button to start capturing a route.") + ) + } else { + ForEach(store.routes) { route in + routeRow(route) + } + .onDelete { indexSet in + indexSet.map { store.routes[$0] }.forEach { store.delete($0) } + } + } + } + } + + private func routeRow(_ route: SavedRoute) -> some View { + VStack(alignment: .leading, spacing: 4) { + Text(route.name).font(.headline) + HStack(spacing: 12) { + Label(formatDate(route.date), systemImage: "calendar") + Label(formatDuration(route.durationSeconds), systemImage: "clock") + Label(formatDistance(route.distanceMetres), systemImage: "ruler") + } + .font(.caption).foregroundStyle(.secondary) + if !route.waypoints.isEmpty { + Label("\(route.waypoints.count) waypoints", systemImage: "mappin.and.ellipse") + .font(.caption2).foregroundStyle(.secondary) + } + } + .padding(.vertical, 4) + } + + // MARK: - Recording view + + private func recordingView(_ rec: RouteRecorder) -> some View { + VStack(spacing: 0) { + // Live stats bar + HStack(spacing: 4) { + statBadge(value: rec.elapsedString, label: "elapsed", icon: "clock.fill", color: .red) + statBadge(value: "\(rec.points.count)", label: "points", icon: "location", color: .blue) + statBadge(value: formatDistance(rec.distanceSoFar), label: "dist", icon: "ruler", color: .green) + statBadge(value: "\(rec.waypoints.count)", label: "waypoints", icon: "mappin", color: .orange) + } + .padding(.horizontal, 8) + .padding(.vertical, 10) + .background(.ultraThinMaterial) + + Divider() + + RecordingMapView(recorder: rec) + .frame(maxHeight: .infinity) + + Divider() + + // Controls + HStack(spacing: 16) { + Button { showWaypointSheet = true } label: { + Label("Waypoint", systemImage: "mappin.and.ellipse") + .frame(maxWidth: .infinity) + } + .buttonStyle(.bordered) + .controlSize(.large) + + Button(role: .destructive) { + if let route = rec.stopRecording() { + pendingRoute = route + routeName = route.name + showSaveSheet = true + } + } label: { + Label("Stop", systemImage: "stop.circle.fill") + .frame(maxWidth: .infinity) + } + .buttonStyle(.borderedProminent) + .tint(.red) + .controlSize(.large) + } + .padding() + } + } + + private func statBadge(value: String, label: String, icon: String, color: Color) -> some View { + VStack(spacing: 2) { + Label(value, systemImage: icon) + .font(.system(.footnote, design: .monospaced).bold()) + .foregroundStyle(color) + .lineLimit(1) + Text(label).font(.caption2).foregroundStyle(.secondary) + } + .frame(maxWidth: .infinity) + } + + // MARK: - Waypoint sheet + + private var waypointSheet: some View { + NavigationStack { + Form { + Section("Label") { + TextField("e.g. Gate, Bench, Corner", text: $waypointLabel) + } + Section("Robot action at this point") { + Picker("Action", selection: $waypointAction) { + ForEach(WaypointAction.allCases) { action in + Label(action.displayName, systemImage: action.systemImage).tag(action) + } + } + .pickerStyle(.inline) + .labelsHidden() + } + } + .navigationTitle("Add Waypoint") + .navigationBarTitleDisplayMode(.inline) + .toolbar { + ToolbarItem(placement: .cancellationAction) { + Button("Cancel") { showWaypointSheet = false } + } + ToolbarItem(placement: .confirmationAction) { + Button("Add") { + recorder?.addWaypoint( + label: waypointLabel.isEmpty ? "Waypoint" : waypointLabel, + action: waypointAction + ) + waypointLabel = "" + waypointAction = .none + showWaypointSheet = false + } + } + } + } + .presentationDetents([.medium]) + } + + // MARK: - Save sheet + + private func saveRouteSheet(_ route: SavedRoute) -> some View { + NavigationStack { + Form { + Section("Route name") { + TextField("Name", text: $routeName) + } + Section("Summary") { + LabeledContent("Points", value: "\(route.points.count)") + LabeledContent("Duration", value: formatDuration(route.durationSeconds)) + LabeledContent("Distance", value: formatDistance(route.distanceMetres)) + LabeledContent("Waypoints", value: "\(route.waypoints.count)") + } + } + .navigationTitle("Save Route") + .navigationBarTitleDisplayMode(.inline) + .toolbar { + ToolbarItem(placement: .cancellationAction) { + Button("Discard") { pendingRoute = nil; showSaveSheet = false } + .foregroundStyle(.red) + } + ToolbarItem(placement: .confirmationAction) { + Button("Save") { + if var r = pendingRoute { + if !routeName.isEmpty { r.name = routeName } + store.save(r) + } + pendingRoute = nil; showSaveSheet = false + } + } + } + } + .presentationDetents([.medium]) + } + + // MARK: - Formatters + + private func formatDate(_ date: Date) -> String { + let f = DateFormatter(); f.dateStyle = .short; f.timeStyle = .short + return f.string(from: date) + } + + private func formatDuration(_ s: Double) -> String { + let t = Int(s) + if t < 60 { return "\(t)s" } + if t < 3600 { return "\(t/60)m \(t%60)s" } + return "\(t/3600)h \((t%3600)/60)m" + } + + private func formatDistance(_ m: Double) -> String { + m < 1000 ? String(format: "%.0f m", m) : String(format: "%.2f km", m / 1000) + } +} + +// MARK: - Live recording map (UIKit bridge) + +private struct RecordingMapView: UIViewRepresentable { + @ObservedObject var recorder: RouteRecorder + + func makeUIView(context: Context) -> MKMapView { + let map = MKMapView() + map.showsUserLocation = true + map.userTrackingMode = .follow + map.delegate = context.coordinator + return map + } + + func updateUIView(_ map: MKMapView, context: Context) { + map.removeOverlays(map.overlays) + map.removeAnnotations(map.annotations.filter { !($0 is MKUserLocation) }) + + let coords = recorder.points.map { + CLLocationCoordinate2D(latitude: $0.latitude, longitude: $0.longitude) + } + if coords.count > 1 { + map.addOverlay(MKPolyline(coordinates: coords, count: coords.count)) + } + + for wp in recorder.waypoints { + map.addAnnotation(WaypointAnnotation(waypoint: wp)) + } + } + + func makeCoordinator() -> Coordinator { Coordinator() } + + final class Coordinator: NSObject, MKMapViewDelegate { + func mapView(_ mapView: MKMapView, rendererFor overlay: MKOverlay) -> MKOverlayRenderer { + guard let line = overlay as? MKPolyline else { + return MKOverlayRenderer(overlay: overlay) + } + let r = MKPolylineRenderer(polyline: line) + r.strokeColor = .systemRed; r.lineWidth = 3 + return r + } + } +} + +private final class WaypointAnnotation: NSObject, MKAnnotation { + let coordinate: CLLocationCoordinate2D + let title: String? + let subtitle: String? + init(waypoint: Waypoint) { + coordinate = CLLocationCoordinate2D(latitude: waypoint.latitude, + longitude: waypoint.longitude) + title = waypoint.label + subtitle = waypoint.action == .none ? nil : waypoint.action.displayName + } +} + +#Preview { + RoutesView().environmentObject(SensorManager()) +} diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 32dfc51..47e3d63 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -107,7 +107,7 @@ final class SensorManager: NSObject, ObservableObject { // MARK: - Internal sensor state - private var lastKnownLocation: CLLocation? + private(set) var lastKnownLocation: CLLocation? private var lastKnownMotion: CMDeviceMotion? /// Orin-fused phone absolute position (RTK GPS + UWB offset). -- 2.47.2 From 615dc405d0f4633b880247c1f4dc02bf0f5c03a5 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 20:01:47 -0400 Subject: [PATCH 17/19] =?UTF-8?q?fix:=20auto-connect=20to=20UWB=5FTAG=20on?= =?UTF-8?q?=20launch=20=E2=80=94=20no=20user=20action=20needed?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Root cause: autoReconnect was false on init, so the centralManagerDidUpdateState(.poweredOn) callback was a no-op. User had to tap 'Scan' to set autoReconnect=true and start scanning. Fixes: - Set autoReconnect=true before creating CBCentralManager so the .poweredOn callback immediately starts scanning on every app launch - Scan timeout (15s): on expiry, call reconnectAfterDelay() instead of staying idle — retries every 2s until TAG is found (handles TAG still booting after firmware flash) Behaviour after this change: Launch → auto-scan within ~1s of Bluetooth ready TAG not found after 15s → retry after 2s, repeat indefinitely TAG disconnects → rescan after 2s TAG reboots/reflashes → found within one 15s scan window User taps Disconnect → autoReconnect=false, stops all retries Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/BLEManager.swift | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/SulTee/SulTee/BLEManager.swift b/SulTee/SulTee/BLEManager.swift index 5dd3906..53c3ef3 100644 --- a/SulTee/SulTee/BLEManager.swift +++ b/SulTee/SulTee/BLEManager.swift @@ -42,6 +42,10 @@ final class BLEManager: NSObject, ObservableObject { override init() { super.init() + // autoReconnect=true before creating the central so that + // centralManagerDidUpdateState(.poweredOn) immediately starts scanning + // without any user action. The user can still tap Disconnect to stop. + autoReconnect = true central = CBCentralManager(delegate: self, queue: DispatchQueue(label: "ble.queue", qos: .utility)) } @@ -95,12 +99,13 @@ final class BLEManager: NSObject, ObservableObject { // We filter by device name prefix in didDiscover instead. central.scanForPeripherals(withServices: nil, options: [CBCentralManagerScanOptionAllowDuplicatesKey: false]) - // Auto-stop after 15 s if nothing found + // Stop after 15 s if nothing found, then retry after 5 s (TAG may be booting) 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 } + self.reconnectAfterDelay() } } -- 2.47.2 From f954b844d405734e72c661b762e105c5e7f891e8 Mon Sep 17 00:00:00 2001 From: sl-ios Date: Mon, 6 Apr 2026 20:47:30 -0400 Subject: [PATCH 18/19] =?UTF-8?q?feat:=20Add=20Pilot=20tab=20=E2=80=94=20M?= =?UTF-8?q?JPEG=20camera=20feed=20+=20virtual=20joystick=20RC=20control?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - New PilotView.swift: full-screen MJPEG stream via WKWebView, virtual joystick overlay (bottom-right, semi-transparent), camera switcher pill (top-right, hidden when single source) - Dead-man switch: finger lift snaps joystick to zero; next 10 Hz tick publishes {linear_x:0, angular_z:0} within 100 ms - cmd_vel published to saltybot/cmd_vel at 10 Hz max via MQTT - SensorManager: add ensureMQTTConnected(), releaseMQTTIfIdle(), publishCmdVel(linearX:angularZ:) so Pilot tab can use MQTT independently of Follow-Me streaming - ContentView: add Pilot tab (camera.fill icon, last tab) - xcodeproj: register PilotView.swift in Sources build phase Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee.xcodeproj/project.pbxproj | 4 + SulTee/SulTee/ContentView.swift | 2 + SulTee/SulTee/PilotView.swift | 187 ++++++++++++++++++++++++ SulTee/SulTee/SensorManager.swift | 20 +++ 4 files changed, 213 insertions(+) create mode 100644 SulTee/SulTee/PilotView.swift diff --git a/SulTee/SulTee.xcodeproj/project.pbxproj b/SulTee/SulTee.xcodeproj/project.pbxproj index 67a3785..6092692 100644 --- a/SulTee/SulTee.xcodeproj/project.pbxproj +++ b/SulTee/SulTee.xcodeproj/project.pbxproj @@ -23,6 +23,7 @@ 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 */; }; + A100000100000000000014AA /* PilotView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000014AB /* PilotView.swift */; }; /* End PBXBuildFile section */ /* Begin PBXFileReference section */ @@ -44,6 +45,7 @@ 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 = ""; }; + A100000100000000000014AB /* PilotView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = PilotView.swift; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ @@ -83,6 +85,7 @@ A100000100000000000011BB /* RouteStore.swift */, A100000100000000000012BB /* RouteRecorder.swift */, A100000100000000000013BB /* RoutesView.swift */, + A100000100000000000014AB /* PilotView.swift */, A100000100000000000005AB /* Assets.xcassets */, A100000100000000000006AB /* Info.plist */, ); @@ -182,6 +185,7 @@ A100000100000000000011AB /* RouteStore.swift in Sources */, A100000100000000000012AB /* RouteRecorder.swift in Sources */, A100000100000000000013AB /* RoutesView.swift in Sources */, + A100000100000000000014AA /* PilotView.swift in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/SulTee/SulTee/ContentView.swift b/SulTee/SulTee/ContentView.swift index 35f3ba1..17f8556 100644 --- a/SulTee/SulTee/ContentView.swift +++ b/SulTee/SulTee/ContentView.swift @@ -15,6 +15,8 @@ struct ContentView: View { .tabItem { Label("BLE Tag", systemImage: "dot.radiowaves.right") } RoutesView() .tabItem { Label("Routes", systemImage: "point.bottomleft.forward.to.point.topright.scurvepath") } + PilotView() + .tabItem { Label("Pilot", systemImage: "camera.fill") } } } } diff --git a/SulTee/SulTee/PilotView.swift b/SulTee/SulTee/PilotView.swift new file mode 100644 index 0000000..b2746c6 --- /dev/null +++ b/SulTee/SulTee/PilotView.swift @@ -0,0 +1,187 @@ +import SwiftUI +import WebKit + +// MARK: - Camera source + +struct CameraSource: Identifiable, Hashable { + let id: String + let name: String + let url: String +} + +// MARK: - MJPEG stream renderer (WKWebView) + +/// Renders an MJPEG stream URL inside a black-background WKWebView. +/// Reloads only when `urlString` changes (guarded by Coordinator). +struct MJPEGStreamView: UIViewRepresentable { + let urlString: String + + class Coordinator { + var loadedURL = "" + } + + func makeCoordinator() -> Coordinator { Coordinator() } + + func makeUIView(context: Context) -> WKWebView { + let cfg = WKWebViewConfiguration() + cfg.allowsInlineMediaPlayback = true + let wv = WKWebView(frame: .zero, configuration: cfg) + wv.backgroundColor = .black + wv.isOpaque = true + wv.scrollView.isScrollEnabled = false + wv.scrollView.bounces = false + return wv + } + + func updateUIView(_ wv: WKWebView, context: Context) { + guard context.coordinator.loadedURL != urlString else { return } + context.coordinator.loadedURL = urlString + let html = """ + + + + + + + """ + wv.loadHTMLString(html, baseURL: nil) + } +} + +// MARK: - Virtual joystick + +/// Single-touch virtual joystick. Knob snaps back to center on release (dead-man). +/// `value.x` ∈ [-1, 1] (right positive), `value.y` ∈ [-1, 1] (down positive). +struct JoystickControl: View { + @Binding var value: CGPoint + + private let baseRadius: CGFloat = 64 + private let knobRadius: CGFloat = 24 + + @State private var offset: CGSize = .zero + + var body: some View { + let travel = baseRadius - knobRadius + let cx = min(max(offset.width, -travel), travel) + let cy = min(max(offset.height, -travel), travel) + + ZStack { + // Base ring + Circle() + .fill(.white.opacity(0.12)) + .frame(width: baseRadius * 2, height: baseRadius * 2) + .overlay(Circle().stroke(.white.opacity(0.35), lineWidth: 1.5)) + + // Knob + Circle() + .fill(.white.opacity(offset == .zero ? 0.35 : 0.70)) + .frame(width: knobRadius * 2, height: knobRadius * 2) + .offset(x: cx, y: cy) + } + .gesture( + DragGesture(minimumDistance: 0) + .onChanged { g in + offset = g.translation + let travel = baseRadius - knobRadius + value = CGPoint( + x: min(max(g.translation.width / travel, -1), 1), + y: min(max(g.translation.height / travel, -1), 1) + ) + } + .onEnded { _ in + // Dead-man: finger lifted → snap to zero immediately. + // The cmd timer will publish zeros on the next tick (≤ 100 ms). + offset = .zero + value = .zero + } + ) + } +} + +// MARK: - Pilot view + +struct PilotView: View { + @EnvironmentObject var sensor: SensorManager + + private static let cameras: [CameraSource] = [ + CameraSource(id: "realsense", name: "RealSense", url: "http://100.64.0.2:8888/stream"), + // Add more cameras here: CameraSource(id: "...", name: "...", url: "...") + ] + + @State private var selectedCamIndex = 0 + @State private var joystick: CGPoint = .zero + + /// 10 Hz publish timer — rate-limits cmd_vel to Orin. + private let cmdTimer = Timer.publish(every: 0.1, on: .main, in: .common).autoconnect() + + var body: some View { + ZStack(alignment: .bottomTrailing) { + Color.black.ignoresSafeArea() + + // Full-screen camera feed + MJPEGStreamView(urlString: Self.cameras[selectedCamIndex].url) + .ignoresSafeArea() + + // Camera switcher pill — top-right (hidden when only one source) + if Self.cameras.count > 1 { + VStack { + HStack { + Spacer() + cameraSwitcher + .padding(.top, 56) + .padding(.trailing, 16) + } + Spacer() + } + } + + // Joystick overlay — bottom-right, semi-transparent + JoystickControl(value: $joystick) + .padding(.trailing, 32) + .padding(.bottom, 52) + } + // Publish at exactly 10 Hz — joystick value is zero when not touched. + .onReceive(cmdTimer) { _ in + sensor.publishCmdVel( + linearX: Float(-joystick.y), // drag up → positive linear_x (forward) + angularZ: Float(-joystick.x) // drag left → positive angular_z (counter-cw) + ) + } + .onAppear { sensor.ensureMQTTConnected() } + .onDisappear { + // Safety: publish a stop command and release MQTT if Follow-Me isn't using it. + joystick = .zero + sensor.publishCmdVel(linearX: 0, angularZ: 0) + sensor.releaseMQTTIfIdle() + } + } + + private var cameraSwitcher: some View { + Menu { + ForEach(Array(Self.cameras.enumerated()), id: \.element.id) { i, cam in + Button { + selectedCamIndex = i + } label: { + if selectedCamIndex == i { + Label(cam.name, systemImage: "checkmark") + } else { + Text(cam.name) + } + } + } + } label: { + Label(Self.cameras[selectedCamIndex].name, + systemImage: "camera.on.rectangle.fill") + .font(.caption.bold()) + .padding(.horizontal, 12) + .padding(.vertical, 7) + .background(.ultraThinMaterial, in: Capsule()) + } + .foregroundStyle(.white) + } +} diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 47e3d63..96959cf 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -242,6 +242,26 @@ final class SensorManager: NSObject, ObservableObject { followPreset = preset; publishFollowPreset() } + // MARK: - MQTT: Pilot tab lifecycle + cmd_vel + + /// Ensure MQTT is open. Safe to call even when Follow-Me is already streaming. + func ensureMQTTConnected() { + mqtt.connect() // no-op if state != .disconnected (guarded inside MQTTClient) + } + + /// Tear down MQTT only if Follow-Me is not running. + func releaseMQTTIfIdle() { + guard !isStreaming else { return } + mqtt.disconnect() + } + + /// Publish a ROS-compatible Twist cmd_vel command at QoS 0. + func publishCmdVel(linearX: Float, angularZ: Float) { + let payload = String(format: "{\"linear_x\":%.3f,\"angular_z\":%.3f}", + linearX, angularZ) + mqtt.publish(topic: "saltybot/cmd_vel", payload: payload) + } + // MARK: - Sensor lifecycle private func ensureSensorsRunning() { -- 2.47.2 From e0c88983f131120dc7aff56047295374ba3f8bcd Mon Sep 17 00:00:00 2001 From: sl-ios Date: Tue, 7 Apr 2026 08:32:10 -0400 Subject: [PATCH 19/19] =?UTF-8?q?feat:=20Pilot=20tab=20=E2=80=94=20HUD=20c?= =?UTF-8?q?ompass=20tape=20from=20saltybot/imu=20heading?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - CompassTapeHUD: fighter-jet style horizontal scrolling tape, smooth sub-pixel phase correction, cardinal labels (N/E/S/W) in yellow, intercardinals (NE/SE/SW/NW), numeric ticks every 30°; fixed yellow ▼ centre indicator; degree readout (e.g. 327°) - Toggle button (top-left, waveform icon) shows/hides HUD with fade+slide animation; defaults to on; yellow when active - "NO IMU" placeholder when saltybot/imu data not yet received - SensorManager: subscribe saltybot/imu in startStreaming() and ensureMQTTConnected(); handleBotIMU() parses heading / true_heading (degrees) or yaw (auto-detects radians vs degrees); normalises to [0, 360); exposes as @Published var botHeadingDeg: Double? Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/PilotView.swift | 179 ++++++++++++++++++++++++++++-- SulTee/SulTee/SensorManager.swift | 40 ++++++- 2 files changed, 208 insertions(+), 11 deletions(-) diff --git a/SulTee/SulTee/PilotView.swift b/SulTee/SulTee/PilotView.swift index b2746c6..71cd97b 100644 --- a/SulTee/SulTee/PilotView.swift +++ b/SulTee/SulTee/PilotView.swift @@ -53,6 +53,133 @@ struct MJPEGStreamView: UIViewRepresentable { } } +// MARK: - Fighter-jet compass tape HUD + +/// Horizontal scrolling compass tape — fighter-jet / HUD style. +/// Current heading is always centred; cardinal labels glow yellow. +struct CompassTapeHUD: View { + let heading: Double // degrees, 0–360 + + // Layout constants + private let tapeHeight: CGFloat = 52 + private let pixPerDeg: CGFloat = 3.6 // horizontal pixels per degree + + var body: some View { + ZStack { + // Frosted dark background + RoundedRectangle(cornerRadius: 8) + .fill(.black.opacity(0.50)) + .overlay( + RoundedRectangle(cornerRadius: 8) + .stroke(.white.opacity(0.15), lineWidth: 0.5) + ) + + Canvas { ctx, size in + drawTape(ctx: ctx, size: size) + } + .clipped() + + // Fixed centre triangle — points downward from the top edge + VStack(spacing: 0) { + Image(systemName: "arrowtriangle.down.fill") + .font(.system(size: 9, weight: .bold)) + .foregroundStyle(.yellow) + .shadow(color: .yellow.opacity(0.6), radius: 4) + Spacer() + } + .padding(.top, 1) + + // Heading readout centred at bottom of tape + VStack { + Spacer() + Text(String(format: "%03.0f°", heading)) + .font(.system(size: 12, weight: .bold, design: .monospaced)) + .foregroundStyle(.yellow) + .shadow(color: .black, radius: 2) + } + .padding(.bottom, 3) + } + .frame(height: tapeHeight) + } + + // MARK: Canvas drawing + + private func drawTape(ctx: GraphicsContext, size: CGSize) { + let midX = size.width / 2 + let tickY: CGFloat = 10 // top of tick marks + + // Snap to nearest whole degree for stable per-step grid + let hdgInt = Int(heading.rounded()) + // Sub-pixel phase correction so the tape glides smoothly + let phase = heading - Double(hdgInt) // fractional remainder + + // Draw ±80° around current heading in 5° steps + for offset in stride(from: -80, through: 80, by: 5) { + let actualDeg = ((hdgInt + offset) % 360 + 360) % 360 + let x = midX + (CGFloat(offset) - CGFloat(phase)) * pixPerDeg + + // Skip ticks outside the visible area + guard x >= 0 && x <= size.width else { continue } + + let isCardinal = actualDeg % 90 == 0 + let isIntercardinal = actualDeg % 45 == 0 + let is30 = actualDeg % 30 == 0 + + let tickH: CGFloat + let lineW: CGFloat + let alpha: Double + if isCardinal { + tickH = 18; lineW = 2.0; alpha = 1.0 + } else if isIntercardinal { + tickH = 13; lineW = 1.5; alpha = 0.85 + } else { + tickH = 8; lineW = 1.0; alpha = 0.55 + } + + // Tick mark + var tick = Path() + tick.move(to: CGPoint(x: x, y: tickY)) + tick.addLine(to: CGPoint(x: x, y: tickY + tickH)) + ctx.stroke(tick, with: .color(.white.opacity(alpha)), lineWidth: lineW) + + // Label for cardinals, intercardinals, and every 30° + if isCardinal || isIntercardinal || is30 { + let label = compassLabel(actualDeg) + let color: Color = isCardinal ? .yellow : .white.opacity(0.85) + let size12: CGFloat = isCardinal ? 12 : 9.5 + let weight: Font.Weight = isCardinal ? .bold : .semibold + ctx.draw( + Text(label) + .font(.system(size: size12, weight: weight, design: .monospaced)) + .foregroundStyle(color), + at: CGPoint(x: x, y: tickY + tickH + 9), + anchor: .top + ) + } + } + + // Subtle centre guide line (vertical) + var guide = Path() + guide.move(to: CGPoint(x: midX, y: tickY)) + guide.addLine(to: CGPoint(x: midX, y: tickY + 18)) + ctx.stroke(guide, with: .color(.yellow.opacity(0.25)), lineWidth: 1) + } + + private func compassLabel(_ deg: Int) -> String { + switch deg { + case 0: return "N" + case 45: return "NE" + case 90: return "E" + case 135: return "SE" + case 180: return "S" + case 225: return "SW" + case 270: return "W" + case 315: return "NW" + default: return "\(deg)" + } + } +} + // MARK: - Virtual joystick /// Single-touch virtual joystick. Knob snaps back to center on release (dead-man). @@ -115,6 +242,7 @@ struct PilotView: View { @State private var selectedCamIndex = 0 @State private var joystick: CGPoint = .zero + @State private var showHUD = true /// 10 Hz publish timer — rate-limits cmd_vel to Orin. private let cmdTimer = Timer.publish(every: 0.1, on: .main, in: .common).autoconnect() @@ -127,17 +255,48 @@ struct PilotView: View { MJPEGStreamView(urlString: Self.cameras[selectedCamIndex].url) .ignoresSafeArea() - // Camera switcher pill — top-right (hidden when only one source) - if Self.cameras.count > 1 { - VStack { - HStack { - Spacer() - cameraSwitcher - .padding(.top, 56) - .padding(.trailing, 16) + // ── Top overlay bar ───────────────────────────────────────────── + VStack(spacing: 0) { + HStack(alignment: .top, spacing: 8) { + + // HUD toggle — top-left + Button { + withAnimation(.easeInOut(duration: 0.2)) { showHUD.toggle() } + } label: { + Image(systemName: showHUD ? "waveform.path.ecg.rectangle.fill" + : "waveform.path.ecg.rectangle") + .font(.system(size: 15, weight: .semibold)) + .foregroundStyle(showHUD ? .yellow : .white.opacity(0.7)) + .padding(8) + .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 8)) + } + + // Compass tape — fills remaining width + if showHUD, let hdg = sensor.botHeadingDeg { + CompassTapeHUD(heading: hdg) + .transition(.opacity.combined(with: .move(edge: .top))) + } else if showHUD { + // Placeholder when no heading data yet + RoundedRectangle(cornerRadius: 8) + .fill(.black.opacity(0.45)) + .frame(height: 52) + .overlay( + Text("NO IMU") + .font(.system(size: 11, weight: .semibold, design: .monospaced)) + .foregroundStyle(.white.opacity(0.4)) + ) + .transition(.opacity.combined(with: .move(edge: .top))) + } + + // Camera switcher — top-right (hidden when only one source) + if Self.cameras.count > 1 { + cameraSwitcher } - Spacer() } + .padding(.top, 56) + .padding(.horizontal, 16) + + Spacer() } // Joystick overlay — bottom-right, semi-transparent @@ -152,7 +311,7 @@ struct PilotView: View { angularZ: Float(-joystick.x) // drag left → positive angular_z (counter-cw) ) } - .onAppear { sensor.ensureMQTTConnected() } + .onAppear { sensor.ensureMQTTConnected() } .onDisappear { // Safety: publish a stop command and release MQTT if Follow-Me isn't using it. joystick = .zero diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 96959cf..c659adf 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/SulTee/SulTee/SensorManager.swift @@ -58,6 +58,12 @@ final class SensorManager: NSObject, ObservableObject { } @Published var distanceSource: DistanceSource = .gps + // MARK: - Bot IMU heading (saltybot/imu) + + /// Robot's magnetic heading in degrees [0, 360), received via MQTT. + /// Nil until the first message arrives. + @Published var botHeadingDeg: Double? = nil + // MARK: - UWB local data (saltybot/uwb/range + saltybot/uwb/position) @Published var uwbPosition: UWBPosition? = nil @@ -100,6 +106,7 @@ final class SensorManager: NSObject, ObservableObject { 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 botIMUTopic = "saltybot/imu" private static let followModeTopic = "saltybot/follow/mode" private static let followRangeTopic = "saltybot/follow/range" private static let maxBreadcrumbs = 60 @@ -167,6 +174,7 @@ final class SensorManager: NSObject, ObservableObject { case Self.uwbRangeTopic: self.handleUWBRange(payload) case Self.uwbPositionTopic: self.handleUWBPosition(payload) case Self.uwbTagPosTopic: self.handleUWBTagPosition(payload) + case Self.botIMUTopic: self.handleBotIMU(payload) default: break } } @@ -206,6 +214,7 @@ final class SensorManager: NSObject, ObservableObject { mqtt.subscribe(topic: Self.uwbRangeTopic) mqtt.subscribe(topic: Self.uwbPositionTopic) mqtt.subscribe(topic: Self.uwbTagPosTopic) + mqtt.subscribe(topic: Self.botIMUTopic) ensureSensorsRunning() startRateTimer() startMQTTGPSTimer() @@ -244,9 +253,11 @@ final class SensorManager: NSObject, ObservableObject { // MARK: - MQTT: Pilot tab lifecycle + cmd_vel - /// Ensure MQTT is open. Safe to call even when Follow-Me is already streaming. + /// Ensure MQTT is open and bot IMU topic subscribed. + /// Safe to call even when Follow-Me is already streaming. func ensureMQTTConnected() { mqtt.connect() // no-op if state != .disconnected (guarded inside MQTTClient) + mqtt.subscribe(topic: Self.botIMUTopic) } /// Tear down MQTT only if Follow-Me is not running. @@ -376,6 +387,33 @@ final class SensorManager: NSObject, ObservableObject { updateBestPhonePosition() } + // MARK: - Bot IMU handler + + /// Parse heading from the robot's IMU message (saltybot/imu). + /// Accepts {"heading": }, {"true_heading": }, or {"yaw": }. + /// Yaw is treated as degrees if ≤ 360, radians otherwise (converted × 180/π). + private func handleBotIMU(_ payload: String) { + guard let data = payload.data(using: .utf8), + let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any] + else { return } + + let deg: Double? + if let h = json["heading"] as? Double { + deg = h + } else if let h = json["true_heading"] as? Double { + deg = h + } else if let y = json["yaw"] as? Double { + deg = abs(y) <= 2 * .pi ? y * 180 / .pi : y // radians → degrees if needed + } else { + deg = nil + } + + if let d = deg { + botHeadingDeg = (d.truncatingRemainder(dividingBy: 360) + 360) + .truncatingRemainder(dividingBy: 360) + } + } + // MARK: - Phone position source selection /// Selects the best available phone position: -- 2.47.2