feat: Merge SaltyTag BLE — GPS/IMU streaming to UWB tag, anchor display, UWB position authority #5
67
CLAUDE.md
Normal file
67
CLAUDE.md
Normal file
@ -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-<N>-<slug>`
|
||||
|
||||
## 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: <description> (Issue #N)' \
|
||||
--description '<details>' --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-<N>"`
|
||||
- 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
|
||||
389
SulTee/SulTee.xcodeproj/project.pbxproj
Normal file
389
SulTee/SulTee.xcodeproj/project.pbxproj
Normal file
@ -0,0 +1,389 @@
|
||||
// !$*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 */; };
|
||||
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 */; };
|
||||
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 */; };
|
||||
A100000100000000000014AA /* PilotView.swift in Sources */ = {isa = PBXBuildFile; fileRef = A100000100000000000014AB /* PilotView.swift */; };
|
||||
/* End PBXBuildFile section */
|
||||
|
||||
/* Begin PBXFileReference section */
|
||||
A100000100000000000001AB /* SulTeeApp.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = SulTeeApp.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000002AB /* ContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = ContentView.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000003AB /* SensorManager.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = SensorManager.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000004AB /* WebSocketClient.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = WebSocketClient.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000005AB /* Assets.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; path = Assets.xcassets; sourceTree = "<group>"; };
|
||||
A100000100000000000006AB /* Info.plist */ = {isa = PBXFileReference; lastKnownFileType = text.plist.xml; path = Info.plist; sourceTree = "<group>"; };
|
||||
A100000100000000000007AB /* SulTee.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = SulTee.app; sourceTree = BUILT_PRODUCTS_DIR; };
|
||||
A100000100000000000009AB /* MQTTClient.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MQTTClient.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000AAB /* MapContentView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MapContentView.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000BAB /* UWBModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = UWBModels.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000CAB /* BLEManager.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = BLEManager.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000010BB /* RouteModels.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RouteModels.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000011BB /* RouteStore.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RouteStore.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000012BB /* RouteRecorder.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RouteRecorder.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000013BB /* RoutesView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RoutesView.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000DAB /* BLEPackets.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = BLEPackets.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000EAB /* AnchorInfo.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = AnchorInfo.swift; sourceTree = "<group>"; };
|
||||
A10000010000000000000FAB /* BLEStatusView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = BLEStatusView.swift; sourceTree = "<group>"; };
|
||||
A100000100000000000014AB /* PilotView.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = PilotView.swift; sourceTree = "<group>"; };
|
||||
/* 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 = "<group>";
|
||||
};
|
||||
A100000100000000000011GR /* SulTee */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
A100000100000000000001AB /* SulTeeApp.swift */,
|
||||
A100000100000000000002AB /* ContentView.swift */,
|
||||
A100000100000000000003AB /* SensorManager.swift */,
|
||||
A100000100000000000004AB /* WebSocketClient.swift */,
|
||||
A100000100000000000009AB /* MQTTClient.swift */,
|
||||
A10000010000000000000AAB /* MapContentView.swift */,
|
||||
A10000010000000000000BAB /* UWBModels.swift */,
|
||||
A10000010000000000000CAB /* BLEManager.swift */,
|
||||
A10000010000000000000DAB /* BLEPackets.swift */,
|
||||
A10000010000000000000EAB /* AnchorInfo.swift */,
|
||||
A10000010000000000000FAB /* BLEStatusView.swift */,
|
||||
A100000100000000000010BB /* RouteModels.swift */,
|
||||
A100000100000000000011BB /* RouteStore.swift */,
|
||||
A100000100000000000012BB /* RouteRecorder.swift */,
|
||||
A100000100000000000013BB /* RoutesView.swift */,
|
||||
A100000100000000000014AB /* PilotView.swift */,
|
||||
A100000100000000000005AB /* Assets.xcassets */,
|
||||
A100000100000000000006AB /* Info.plist */,
|
||||
);
|
||||
path = SulTee;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
A100000100000000000012GR /* Products */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
A100000100000000000007AB /* SulTee.app */,
|
||||
);
|
||||
name = Products;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
/* 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 */,
|
||||
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 */,
|
||||
A100000100000000000010AB /* RouteModels.swift in Sources */,
|
||||
A100000100000000000011AB /* RouteStore.swift in Sources */,
|
||||
A100000100000000000012AB /* RouteRecorder.swift in Sources */,
|
||||
A100000100000000000013AB /* RoutesView.swift in Sources */,
|
||||
A100000100000000000014AA /* PilotView.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 */;
|
||||
}
|
||||
7
SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata
generated
Normal file
7
SulTee/SulTee.xcodeproj/project.xcworkspace/contents.xcworkspacedata
generated
Normal file
@ -0,0 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Workspace
|
||||
version = "1.0">
|
||||
<FileRef
|
||||
location = "self:">
|
||||
</FileRef>
|
||||
</Workspace>
|
||||
36
SulTee/SulTee/AnchorInfo.swift
Normal file
36
SulTee/SulTee/AnchorInfo.swift
Normal file
@ -0,0 +1,36 @@
|
||||
import Foundation
|
||||
|
||||
/// A single UWB anchor measurement received via BLE notification.
|
||||
struct AnchorInfo: Identifiable {
|
||||
let id: UInt8
|
||||
let rangeMetres: Double
|
||||
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.
|
||||
/// IDs 0/1 map to Front/Back (HAL two-anchor format); others show "A<n>".
|
||||
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 {
|
||||
rangeMetres < 10
|
||||
? String(format: "%.2f m", rangeMetres)
|
||||
: String(format: "%.1f m", rangeMetres)
|
||||
}
|
||||
|
||||
/// 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"
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,13 @@
|
||||
{
|
||||
"images" : [
|
||||
{
|
||||
"idiom" : "universal",
|
||||
"platform" : "ios",
|
||||
"size" : "1024x1024"
|
||||
}
|
||||
],
|
||||
"info" : {
|
||||
"author" : "xcode",
|
||||
"version" : 1
|
||||
}
|
||||
}
|
||||
6
SulTee/SulTee/Assets.xcassets/Contents.json
Normal file
6
SulTee/SulTee/Assets.xcassets/Contents.json
Normal file
@ -0,0 +1,6 @@
|
||||
{
|
||||
"info" : {
|
||||
"author" : "xcode",
|
||||
"version" : 1
|
||||
}
|
||||
}
|
||||
228
SulTee/SulTee/BLEManager.swift
Normal file
228
SulTee/SulTee/BLEManager.swift
Normal file
@ -0,0 +1,228 @@
|
||||
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()
|
||||
// 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))
|
||||
}
|
||||
|
||||
// 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 }
|
||||
// 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 }
|
||||
let writeType: CBCharacteristicWriteType =
|
||||
c.properties.contains(.writeWithoutResponse) ? .withoutResponse : .withResponse
|
||||
p.writeValue(data, for: c, type: writeType)
|
||||
}
|
||||
|
||||
// MARK: - Internal helpers
|
||||
|
||||
private func doStartScan() {
|
||||
DispatchQueue.main.async { self.connectionState = .scanning }
|
||||
// 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])
|
||||
// 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()
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
// 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
|
||||
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)")
|
||||
}
|
||||
}
|
||||
}
|
||||
201
SulTee/SulTee/BLEPackets.swift
Normal file
201
SulTee/SulTee/BLEPackets.swift
Normal file
@ -0,0 +1,201 @@
|
||||
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
|
||||
//
|
||||
// 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
|
||||
// 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: 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: rssi, ageMs: 0, receivedAt: now),
|
||||
AnchorInfo(id: 1, rangeMetres: Double(backMM) / 1000.0,
|
||||
rssiDBm: rssi, 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])
|
||||
var result: [AnchorInfo] = []
|
||||
|
||||
for i in 0..<count {
|
||||
let base = 1 + i * 9
|
||||
guard base + 9 <= data.count else { break }
|
||||
|
||||
let anchorID = data[base]
|
||||
let rangeMM = data.readInt32LE(at: base + 1)
|
||||
let rssiTimes10 = data.readInt16LE(at: base + 5)
|
||||
let ageMs = data.readUInt16LE(at: base + 7)
|
||||
|
||||
result.append(AnchorInfo(
|
||||
id: anchorID,
|
||||
rangeMetres: Double(rangeMM) / 1000.0,
|
||||
rssiDBm: Double(rssiTimes10) / 10.0,
|
||||
ageMs: ageMs,
|
||||
receivedAt: now
|
||||
))
|
||||
|
||||
}
|
||||
return result
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Data helpers (little-endian read / write)
|
||||
|
||||
private extension Data {
|
||||
mutating func writeInt16LE(_ value: Int16, at offset: Int) {
|
||||
let v = UInt16(bitPattern: value)
|
||||
self[offset] = UInt8(v & 0xFF)
|
||||
self[offset + 1] = UInt8(v >> 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)
|
||||
}
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
192
SulTee/SulTee/BLEStatusView.swift
Normal file
192
SulTee/SulTee/BLEStatusView.swift
Normal file
@ -0,0 +1,192 @@
|
||||
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: Binding(get: { ble.gpsStreamEnabled }, set: { ble.gpsStreamEnabled = $0 })) {
|
||||
Label {
|
||||
VStack(alignment: .leading, spacing: 2) {
|
||||
Text("GPS → Tag")
|
||||
Text("1 Hz · 20 bytes/packet")
|
||||
.font(.caption).foregroundStyle(.secondary)
|
||||
}
|
||||
} icon: {
|
||||
Image(systemName: "location.fill")
|
||||
.foregroundStyle(.blue)
|
||||
}
|
||||
}
|
||||
Toggle(isOn: Binding(get: { ble.imuStreamEnabled }, set: { ble.imuStreamEnabled = $0 })) {
|
||||
Label {
|
||||
VStack(alignment: .leading, spacing: 2) {
|
||||
Text("IMU → Tag")
|
||||
Text("2 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) {
|
||||
if let rssi = anchor.rssiString {
|
||||
Text(rssi)
|
||||
.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())
|
||||
}
|
||||
223
SulTee/SulTee/ContentView.swift
Normal file
223
SulTee/SulTee/ContentView.swift
Normal file
@ -0,0 +1,223 @@
|
||||
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") }
|
||||
BLEStatusView()
|
||||
.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") }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
var body: some View {
|
||||
NavigationStack {
|
||||
VStack(spacing: 24) {
|
||||
connectionBanner
|
||||
orinURLField
|
||||
Divider()
|
||||
sensorRatesGrid
|
||||
Divider()
|
||||
if let dist = sensor.distanceToRobot {
|
||||
distanceRow(dist)
|
||||
}
|
||||
Spacer()
|
||||
followMeButton
|
||||
}
|
||||
.padding()
|
||||
.navigationTitle("SAUL-T-MOTE")
|
||||
.onAppear { editingURL = orinURL }
|
||||
}
|
||||
}
|
||||
|
||||
// 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) {
|
||||
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)
|
||||
}
|
||||
|
||||
// MARK: Sensor rates grid
|
||||
|
||||
private var sensorRatesGrid: some View {
|
||||
Grid(horizontalSpacing: 20, verticalSpacing: 12) {
|
||||
GridRow {
|
||||
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)
|
||||
rateCell(icon: "barometer", label: "Baro", rate: sensor.baroRate)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private func rateCell(icon: String, label: String, rate: Double) -> some View {
|
||||
VStack(spacing: 4) {
|
||||
Image(systemName: icon)
|
||||
.font(.title2)
|
||||
.foregroundStyle(rate > 0 ? .green : .secondary)
|
||||
Text(label)
|
||||
.font(.caption)
|
||||
.foregroundStyle(.secondary)
|
||||
Text("\(Int(rate)) Hz")
|
||||
.font(.title3.monospacedDigit())
|
||||
.bold()
|
||||
}
|
||||
.frame(maxWidth: .infinity)
|
||||
.padding()
|
||||
.background(.quaternary, in: RoundedRectangle(cornerRadius: 12))
|
||||
}
|
||||
|
||||
// MARK: Distance row
|
||||
|
||||
private func distanceRow(_ dist: Double) -> some View {
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: Follow-Me button
|
||||
|
||||
private var followMeButton: some View {
|
||||
Button {
|
||||
sensor.isStreaming ? sensor.stopStreaming() : 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 — \(orinURL)"
|
||||
case .connecting: return "Connecting…"
|
||||
case .disconnected: return "Disconnected"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#Preview {
|
||||
ContentView()
|
||||
.environmentObject(SensorManager())
|
||||
}
|
||||
83
SulTee/SulTee/Info.plist
Normal file
83
SulTee/SulTee/Info.plist
Normal file
@ -0,0 +1,83 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
|
||||
<plist version="1.0">
|
||||
<dict>
|
||||
<key>CFBundleDevelopmentRegion</key>
|
||||
<string>$(DEVELOPMENT_LANGUAGE)</string>
|
||||
<key>CFBundleExecutable</key>
|
||||
<string>$(EXECUTABLE_NAME)</string>
|
||||
<key>CFBundleIdentifier</key>
|
||||
<string>$(PRODUCT_BUNDLE_IDENTIFIER)</string>
|
||||
<key>CFBundleInfoDictionaryVersion</key>
|
||||
<string>6.0</string>
|
||||
<key>CFBundleDisplayName</key>
|
||||
<string>SAUL-T-MOTE</string>
|
||||
<key>CFBundleName</key>
|
||||
<string>$(PRODUCT_NAME)</string>
|
||||
<key>CFBundlePackageType</key>
|
||||
<string>$(PRODUCT_BUNDLE_PACKAGE_TYPE)</string>
|
||||
<key>CFBundleShortVersionString</key>
|
||||
<string>1.0</string>
|
||||
<key>CFBundleVersion</key>
|
||||
<string>1</string>
|
||||
<key>LSRequiresIPhoneOS</key>
|
||||
<true/>
|
||||
<key>NSLocationAlwaysAndWhenInUseUsageDescription</key>
|
||||
<string>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.</string>
|
||||
<key>NSLocationWhenInUseUsageDescription</key>
|
||||
<string>Sul-Tee streams your GPS location to SaltyBot for follow-me mode.</string>
|
||||
<key>NSBluetoothAlwaysUsageDescription</key>
|
||||
<string>SAUL-T-MOTE connects to the SaltyBot UWB tag via Bluetooth to stream GPS and IMU data and receive anchor ranging measurements.</string>
|
||||
<key>NSMotionUsageDescription</key>
|
||||
<string>SAUL-T-MOTE streams IMU (accelerometer, gyroscope, magnetometer) to the SaltyBot UWB tag and Orin for follow-me stabilization.</string>
|
||||
<key>UIBackgroundModes</key>
|
||||
<array>
|
||||
<string>location</string>
|
||||
<string>external-accessory</string>
|
||||
<string>bluetooth-central</string>
|
||||
</array>
|
||||
<key>UIDeviceFamily</key>
|
||||
<array>
|
||||
<integer>1</integer>
|
||||
</array>
|
||||
<key>UILaunchStoryboardName</key>
|
||||
<string>LaunchScreen</string>
|
||||
<key>UIRequiredDeviceCapabilities</key>
|
||||
<array>
|
||||
<string>armv7</string>
|
||||
</array>
|
||||
<key>UISupportedInterfaceOrientations</key>
|
||||
<array>
|
||||
<string>UIInterfaceOrientationPortrait</string>
|
||||
<string>UIInterfaceOrientationPortraitUpsideDown</string>
|
||||
<string>UIInterfaceOrientationLandscapeLeft</string>
|
||||
<string>UIInterfaceOrientationLandscapeRight</string>
|
||||
</array>
|
||||
<key>NSAppTransportSecurity</key>
|
||||
<dict>
|
||||
<!-- Allow plain ws:// to Tailscale (CGNAT 100.64.0.0/10) and other local addresses.
|
||||
Error -1022 occurs because ATS blocks non-TLS connections by default.
|
||||
NSAllowsLocalNetworking covers loopback, link-local, and CGNAT ranges. -->
|
||||
<key>NSAllowsLocalNetworking</key>
|
||||
<true/>
|
||||
<key>NSExceptionDomains</key>
|
||||
<dict>
|
||||
<key>100.64.0.2</key>
|
||||
<dict>
|
||||
<key>NSExceptionAllowsInsecureHTTPLoads</key>
|
||||
<true/>
|
||||
</dict>
|
||||
<key>192.168.87.29</key>
|
||||
<dict>
|
||||
<key>NSExceptionAllowsInsecureHTTPLoads</key>
|
||||
<true/>
|
||||
</dict>
|
||||
</dict>
|
||||
</dict>
|
||||
<key>UIApplicationSceneManifest</key>
|
||||
<dict>
|
||||
<key>UIApplicationSupportsMultipleScenes</key>
|
||||
<false/>
|
||||
</dict>
|
||||
</dict>
|
||||
</plist>
|
||||
240
SulTee/SulTee/MQTTClient.swift
Normal file
240
SulTee/SulTee/MQTTClient.swift
Normal file
@ -0,0 +1,240 @@
|
||||
import Foundation
|
||||
import Network
|
||||
|
||||
/// 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 {
|
||||
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
|
||||
|
||||
/// 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) {
|
||||
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 }
|
||||
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 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()
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
private func scheduleRead() {
|
||||
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 error == nil { self.scheduleRead() }
|
||||
}
|
||||
}
|
||||
|
||||
/// Parse one or more MQTT packets from `data`.
|
||||
private func handleIncoming(_ data: Data) {
|
||||
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..<topicEnd], encoding: .utf8) ?? ""
|
||||
let payload = String(bytes: data[topicEnd..<payloadEnd], encoding: .utf8) ?? ""
|
||||
let t = topic, p = payload
|
||||
DispatchQueue.main.async { self.onMessage?(t, p) }
|
||||
}
|
||||
}
|
||||
|
||||
case 0x90: // SUBACK — ignore
|
||||
break
|
||||
case 0xD0: // PINGRESP — ignore
|
||||
break
|
||||
default:
|
||||
break
|
||||
}
|
||||
|
||||
i = payloadEnd
|
||||
}
|
||||
}
|
||||
|
||||
// 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?.connection?.send(content: Data([0xC0, 0x00]), completion: .idempotent)
|
||||
}
|
||||
t.resume()
|
||||
pingTimer = t
|
||||
}
|
||||
|
||||
// MARK: - MQTT packet builders
|
||||
|
||||
private func sendConnect() {
|
||||
connection?.send(content: buildConnect(), completion: .idempotent)
|
||||
}
|
||||
|
||||
private func buildConnect() -> Data {
|
||||
var payload = Data()
|
||||
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)
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
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 {
|
||||
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)])
|
||||
}
|
||||
}
|
||||
315
SulTee/SulTee/MapContentView.swift
Normal file
315
SulTee/SulTee/MapContentView.swift
Normal file
@ -0,0 +1,315 @@
|
||||
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, fading breadcrumb trails, UWB status, and follow controls.
|
||||
struct MapContentView: View {
|
||||
@EnvironmentObject var sensor: SensorManager
|
||||
|
||||
@State private var position: MapCameraPosition = .automatic
|
||||
@State private var followUser = true
|
||||
|
||||
private var insideFollowRange: Bool {
|
||||
guard let d = sensor.distanceToRobot else { return false }
|
||||
return d <= sensor.followPreset.metres
|
||||
}
|
||||
|
||||
var body: some View {
|
||||
ZStack(alignment: .bottom) {
|
||||
map
|
||||
VStack(spacing: 0) {
|
||||
Spacer()
|
||||
followControls
|
||||
statsBar
|
||||
}
|
||||
}
|
||||
.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 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
|
||||
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 followUser = false }
|
||||
.overlay(alignment: .topTrailing) { recenterButton }
|
||||
.overlay(alignment: .topLeading) {
|
||||
VStack(alignment: .leading, spacing: 6) {
|
||||
uwbBadge
|
||||
positionSourceBadge
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - UWB status badge (top-left)
|
||||
|
||||
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: - 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 {
|
||||
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, 56)
|
||||
.padding(.trailing, 16)
|
||||
}
|
||||
|
||||
// MARK: - Follow controls panel
|
||||
|
||||
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: 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)
|
||||
.foregroundStyle(.secondary)
|
||||
}
|
||||
}
|
||||
.padding(.horizontal, 20)
|
||||
.padding(.vertical, 12)
|
||||
.background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 16))
|
||||
.padding(.bottom, 24)
|
||||
.padding(.horizontal, 16)
|
||||
}
|
||||
|
||||
// MARK: - Helpers
|
||||
|
||||
private func statCell(value: String,
|
||||
label: String,
|
||||
icon: String,
|
||||
tint: Color = .primary) -> some View {
|
||||
HStack(spacing: 6) {
|
||||
Image(systemName: icon)
|
||||
.foregroundStyle(tint == .primary ? .secondary : tint)
|
||||
.font(.caption)
|
||||
VStack(alignment: .leading, spacing: 0) {
|
||||
Text(value).font(.headline.monospacedDigit()).foregroundStyle(tint)
|
||||
Text(label).font(.caption2).foregroundStyle(.secondary)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private func distanceString(_ m: Double) -> String {
|
||||
m < 1000 ? "\(Int(m)) m" : String(format: "%.1f km", m / 1000)
|
||||
}
|
||||
}
|
||||
|
||||
#Preview {
|
||||
MapContentView()
|
||||
.environmentObject(SensorManager())
|
||||
}
|
||||
346
SulTee/SulTee/PilotView.swift
Normal file
346
SulTee/SulTee/PilotView.swift
Normal file
@ -0,0 +1,346 @@
|
||||
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 = """
|
||||
<!DOCTYPE html><html><head>
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1,user-scalable=no">
|
||||
<style>
|
||||
* { margin: 0; padding: 0; box-sizing: border-box; }
|
||||
body { background: #000; width: 100vw; height: 100vh;
|
||||
display: flex; align-items: center; justify-content: center; overflow: hidden; }
|
||||
img { max-width: 100%; max-height: 100%; object-fit: contain; }
|
||||
</style>
|
||||
</head><body>
|
||||
<img src="\(urlString)">
|
||||
</body></html>
|
||||
"""
|
||||
wv.loadHTMLString(html, baseURL: nil)
|
||||
}
|
||||
}
|
||||
|
||||
// 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).
|
||||
/// `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
|
||||
@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()
|
||||
|
||||
var body: some View {
|
||||
ZStack(alignment: .bottomTrailing) {
|
||||
Color.black.ignoresSafeArea()
|
||||
|
||||
// Full-screen camera feed
|
||||
MJPEGStreamView(urlString: Self.cameras[selectedCamIndex].url)
|
||||
.ignoresSafeArea()
|
||||
|
||||
// ── 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
|
||||
}
|
||||
}
|
||||
.padding(.top, 56)
|
||||
.padding(.horizontal, 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)
|
||||
}
|
||||
}
|
||||
117
SulTee/SulTee/RouteModels.swift
Normal file
117
SulTee/SulTee/RouteModels.swift
Normal file
@ -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..<points.count {
|
||||
let a = CLLocation(latitude: points[i-1].latitude, longitude: points[i-1].longitude)
|
||||
let b = CLLocation(latitude: points[i].latitude, longitude: points[i].longitude)
|
||||
total += a.distance(from: b)
|
||||
}
|
||||
return total
|
||||
}
|
||||
|
||||
// MARK: - MQTT payload for robot route-following (Phase 3)
|
||||
//
|
||||
// Topic: saltybot/route/command
|
||||
// {
|
||||
// "action": "start",
|
||||
// "route_id": "<uuid>",
|
||||
// "route_name":"<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
|
||||
]}
|
||||
]
|
||||
}
|
||||
}
|
||||
112
SulTee/SulTee/RouteRecorder.swift
Normal file
112
SulTee/SulTee/RouteRecorder.swift
Normal file
@ -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..<points.count {
|
||||
let a = CLLocation(latitude: points[i-1].latitude, longitude: points[i-1].longitude)
|
||||
let b = CLLocation(latitude: points[i].latitude, longitude: points[i].longitude)
|
||||
total += a.distance(from: b)
|
||||
}
|
||||
return total
|
||||
}
|
||||
|
||||
var elapsedString: String {
|
||||
let s = Int(elapsedSeconds)
|
||||
return String(format: "%02d:%02d", s / 60, s % 60)
|
||||
}
|
||||
}
|
||||
63
SulTee/SulTee/RouteStore.swift
Normal file
63
SulTee/SulTee/RouteStore.swift
Normal file
@ -0,0 +1,63 @@
|
||||
import Foundation
|
||||
|
||||
/// Persists SavedRoute objects as individual JSON files in the app's Documents directory.
|
||||
final class RouteStore: ObservableObject {
|
||||
|
||||
@Published private(set) var routes: [SavedRoute] = []
|
||||
|
||||
private let directory: URL
|
||||
|
||||
init() {
|
||||
let docs = FileManager.default.urls(for: .documentDirectory, in: .userDomainMask)[0]
|
||||
directory = docs.appendingPathComponent("routes", isDirectory: true)
|
||||
try? FileManager.default.createDirectory(at: directory,
|
||||
withIntermediateDirectories: true)
|
||||
load()
|
||||
}
|
||||
|
||||
// MARK: - Public API
|
||||
|
||||
func save(_ route: SavedRoute) {
|
||||
let url = fileURL(for: route.id)
|
||||
if let data = try? JSONEncoder().encode(route) {
|
||||
try? data.write(to: url, options: .atomic)
|
||||
}
|
||||
if let idx = routes.firstIndex(where: { $0.id == route.id }) {
|
||||
routes[idx] = route
|
||||
} else {
|
||||
routes.append(route)
|
||||
routes.sort { $0.date > $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")
|
||||
}
|
||||
}
|
||||
304
SulTee/SulTee/RoutesView.swift
Normal file
304
SulTee/SulTee/RoutesView.swift
Normal file
@ -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())
|
||||
}
|
||||
620
SulTee/SulTee/SensorManager.swift
Normal file
620
SulTee/SulTee/SensorManager.swift
Normal file
@ -0,0 +1,620 @@
|
||||
import Foundation
|
||||
import CoreLocation
|
||||
import CoreMotion
|
||||
import MapKit
|
||||
import Combine
|
||||
|
||||
// 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
|
||||
|
||||
@Published var isStreaming = false
|
||||
@Published var wsState: WebSocketClient.ConnectionState = .disconnected
|
||||
|
||||
// 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 — fused from UWB-tag or CoreLocation
|
||||
|
||||
/// 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)
|
||||
|
||||
@Published var robotLocation: CLLocationCoordinate2D? = nil
|
||||
@Published var robotBreadcrumbs: [CLLocationCoordinate2D] = []
|
||||
@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: - 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
|
||||
@Published var uwbRanges: [String: UWBRange] = [:]
|
||||
@Published var uwbActive: Bool = false
|
||||
|
||||
// MARK: - Follow settings
|
||||
|
||||
@Published var followMode: FollowMode = .gps
|
||||
@Published var followPreset: FollowPreset = .medium
|
||||
|
||||
// MARK: - BLE tag
|
||||
|
||||
let ble = BLEManager()
|
||||
|
||||
// MARK: - WebSocket config
|
||||
|
||||
static let defaultOrinURL = "wss://www.saultee.bot/ws"
|
||||
private static let orinURLKey = "orinURL"
|
||||
|
||||
private(set) var ws: WebSocketClient
|
||||
|
||||
var orinURLString: String {
|
||||
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: "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 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
|
||||
private static let uwbStaleSeconds = 3.0
|
||||
|
||||
// MARK: - Internal sensor state
|
||||
|
||||
private(set) 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? // 1 Hz BLE GPS write
|
||||
private var bleIMUTimer: Timer? // 2 Hz BLE IMU write
|
||||
private var uwbStalenessTimer: Timer?
|
||||
private var rateTimer: Timer?
|
||||
|
||||
// MARK: - CoreMotion / CoreLocation
|
||||
|
||||
private let locationManager = CLLocationManager()
|
||||
private let motionManager = CMMotionManager()
|
||||
private let altimeter = CMAltimeter()
|
||||
private var cancellables = Set<AnyCancellable>()
|
||||
private var bleCancellables = Set<AnyCancellable>()
|
||||
|
||||
// MARK: - Rate counters
|
||||
|
||||
private var gpsCounts: [Date] = []
|
||||
private var imuCounts: [Date] = []
|
||||
private var headingCounts: [Date] = []
|
||||
private var baroCounts: [Date] = []
|
||||
|
||||
// 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()
|
||||
|
||||
locationManager.delegate = self
|
||||
locationManager.desiredAccuracy = kCLLocationAccuracyBestForNavigation
|
||||
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)
|
||||
|
||||
mqtt.onMessage = { [weak self] topic, payload in
|
||||
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)
|
||||
case Self.uwbTagPosTopic: self.handleUWBTagPosition(payload)
|
||||
case Self.botIMUTopic: self.handleBotIMU(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()
|
||||
// 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)
|
||||
|
||||
func startStreaming() {
|
||||
guard !isStreaming else { return }
|
||||
isStreaming = true
|
||||
ws.connect()
|
||||
mqtt.connect()
|
||||
mqtt.subscribe(topic: Self.robotGPSTopic)
|
||||
mqtt.subscribe(topic: Self.uwbRangeTopic)
|
||||
mqtt.subscribe(topic: Self.uwbPositionTopic)
|
||||
mqtt.subscribe(topic: Self.uwbTagPosTopic)
|
||||
mqtt.subscribe(topic: Self.botIMUTopic)
|
||||
ensureSensorsRunning()
|
||||
startRateTimer()
|
||||
startMQTTGPSTimer()
|
||||
startUWBStalenessTimer()
|
||||
publishFollowMode()
|
||||
publishFollowPreset()
|
||||
}
|
||||
|
||||
func stopStreaming() {
|
||||
guard isStreaming else { return }
|
||||
isStreaming = false
|
||||
ws.disconnect()
|
||||
mqtt.disconnect()
|
||||
// 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
|
||||
}
|
||||
|
||||
func updateURL(_ urlString: String) {
|
||||
guard !isStreaming else { return }
|
||||
orinURLString = urlString
|
||||
if let url = URL(string: urlString), url.scheme?.hasPrefix("ws") == true {
|
||||
ws.url = url
|
||||
}
|
||||
}
|
||||
|
||||
func setFollowMode(_ mode: FollowMode) {
|
||||
followMode = mode; publishFollowMode()
|
||||
}
|
||||
|
||||
func setFollowPreset(_ preset: FollowPreset) {
|
||||
followPreset = preset; publishFollowPreset()
|
||||
}
|
||||
|
||||
// MARK: - MQTT: Pilot tab lifecycle + cmd_vel
|
||||
|
||||
/// 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.
|
||||
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() {
|
||||
locationManager.requestAlwaysAuthorization()
|
||||
locationManager.startUpdatingLocation()
|
||||
locationManager.startUpdatingHeading()
|
||||
if !motionManager.isDeviceMotionActive { startIMU() }
|
||||
if CMAltimeter.isRelativeAltitudeAvailable() { startBarometer() }
|
||||
}
|
||||
|
||||
private func stopSensors() {
|
||||
locationManager.stopUpdatingLocation()
|
||||
locationManager.stopUpdatingHeading()
|
||||
motionManager.stopDeviceMotionUpdates()
|
||||
altimeter.stopRelativeAltitudeUpdates()
|
||||
}
|
||||
|
||||
// MARK: - BLE streaming timers
|
||||
|
||||
private func startBLETimers() {
|
||||
stopBLETimers()
|
||||
// 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 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))
|
||||
}
|
||||
}
|
||||
|
||||
private func stopBLETimers() {
|
||||
bleGPSTimer?.invalidate(); bleGPSTimer = nil
|
||||
bleIMUTimer?.invalidate(); bleIMUTimer = nil
|
||||
}
|
||||
|
||||
// MARK: - MQTT GPS publish (1 Hz)
|
||||
|
||||
private func startMQTTGPSTimer() {
|
||||
mqttGPSTimer = 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.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) {
|
||||
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)
|
||||
updateDistanceToRobot()
|
||||
}
|
||||
|
||||
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())
|
||||
updateDistanceToRobot()
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
/// 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: - Bot IMU handler
|
||||
|
||||
/// Parse heading from the robot's IMU message (saltybot/imu).
|
||||
/// Accepts {"heading": <degrees>}, {"true_heading": <degrees>}, or {"yaw": <value>}.
|
||||
/// 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:
|
||||
/// 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)
|
||||
updateDistanceToRobot()
|
||||
}
|
||||
phonePositionSource = .uwb(accuracyM: uwb.accuracyM)
|
||||
} else if let loc = lastKnownLocation {
|
||||
let coord = loc.coordinate
|
||||
if userLocation != coord {
|
||||
userLocation = coord
|
||||
appendBreadcrumb(coord, to: &userBreadcrumbs)
|
||||
updateDistanceToRobot()
|
||||
}
|
||||
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 }
|
||||
self.uwbRanges = self.uwbRanges.filter { $0.value.timestamp > cutoff }
|
||||
// Re-evaluate phone position source when UWB tag position may have gone stale
|
||||
self.updateBestPhonePosition()
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Breadcrumbs + distance
|
||||
|
||||
private func appendBreadcrumb(_ coord: CLLocationCoordinate2D,
|
||||
to list: inout [CLLocationCoordinate2D]) {
|
||||
list.append(coord)
|
||||
if list.count > Self.maxBreadcrumbs { list.removeFirst() }
|
||||
}
|
||||
|
||||
/// 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
|
||||
|
||||
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",
|
||||
"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],
|
||||
"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, _ 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 }
|
||||
lastKnownLocation = loc
|
||||
recordEvent(in: &gpsCounts)
|
||||
// Let the source-selection logic decide whether to use this or UWB-derived position
|
||||
updateBestPhonePosition()
|
||||
|
||||
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 || ble.isConnected {
|
||||
manager.startUpdatingLocation()
|
||||
manager.startUpdatingHeading()
|
||||
}
|
||||
default: break
|
||||
}
|
||||
}
|
||||
}
|
||||
13
SulTee/SulTee/SulTeeApp.swift
Normal file
13
SulTee/SulTee/SulTeeApp.swift
Normal file
@ -0,0 +1,13 @@
|
||||
import SwiftUI
|
||||
|
||||
@main
|
||||
struct SulTeeApp: App {
|
||||
@StateObject private var sensorManager = SensorManager()
|
||||
|
||||
var body: some Scene {
|
||||
WindowGroup {
|
||||
ContentView()
|
||||
.environmentObject(sensorManager)
|
||||
}
|
||||
}
|
||||
}
|
||||
51
SulTee/SulTee/UWBModels.swift
Normal file
51
SulTee/SulTee/UWBModels.swift
Normal file
@ -0,0 +1,51 @@
|
||||
import Foundation
|
||||
|
||||
// MARK: - Follow mode
|
||||
|
||||
enum FollowMode: String, CaseIterable, Identifiable {
|
||||
case gps = "GPS"
|
||||
case uwb = "UWB"
|
||||
var id: String { rawValue }
|
||||
|
||||
var mqttPayload: String {
|
||||
"{\"mode\":\"\(rawValue.lowercased())\"}"
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Follow range preset
|
||||
|
||||
enum FollowPreset: String, CaseIterable, Identifiable {
|
||||
case close = "Close"
|
||||
case medium = "Medium"
|
||||
case far = "Far"
|
||||
var id: String { rawValue }
|
||||
|
||||
var metres: Double {
|
||||
switch self {
|
||||
case .close: return 1.5
|
||||
case .medium: return 3.0
|
||||
case .far: return 5.0
|
||||
}
|
||||
}
|
||||
|
||||
var mqttPayload: String {
|
||||
let d: [String: Any] = ["range_m": metres, "preset": rawValue.lowercased()]
|
||||
guard let data = try? JSONSerialization.data(withJSONObject: d),
|
||||
let str = String(data: data, encoding: .utf8) else { return "{}" }
|
||||
return str
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - UWB data types
|
||||
|
||||
struct UWBPosition {
|
||||
let x, y, z: Double
|
||||
let timestamp: Date
|
||||
}
|
||||
|
||||
struct UWBRange: Identifiable {
|
||||
let anchorID: String
|
||||
let rangeMetres: Double
|
||||
let timestamp: Date
|
||||
var id: String { anchorID }
|
||||
}
|
||||
114
SulTee/SulTee/WebSocketClient.swift
Normal file
114
SulTee/SulTee/WebSocketClient.swift
Normal file
@ -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
|
||||
|
||||
var 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()
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user