From 00e8a7656c3bf07738adc67e371b527742b0c474 Mon Sep 17 00:00:00 2001 From: pulipakaa24 Date: Sat, 18 Apr 2026 14:15:33 -0400 Subject: [PATCH] Stable tracking of DWM module --- .DS_Store | Bin 0 -> 8196 bytes NearbyDemo.xcodeproj/project.pbxproj | 631 ++++++++++++++++++ .../contents.xcworkspacedata | 7 + NearbyDemo/ARViewContainer.swift | 104 +++ .../AccentColor.colorset/Contents.json | 11 + .../AppIcon.appiconset/Contents.json | 35 + NearbyDemo/Assets.xcassets/Contents.json | 6 + NearbyDemo/ContentView.swift | 117 ++++ NearbyDemo/Info.plist | 17 + NearbyDemo/Managers/ARManager.swift | 92 +++ NearbyDemo/Managers/AnchorEstimator.swift | 191 ++++++ NearbyDemo/Managers/BLEManager.swift | 188 ++++++ NearbyDemo/Managers/NIManager.swift | 248 +++++++ NearbyDemo/Models/QorvoBLEUUIDs.swift | 9 + NearbyDemo/Models/TimestampedPose.swift | 7 + NearbyDemo/Models/TimestampedRange.swift | 6 + NearbyDemo/NearbyDemoApp.swift | 67 ++ NearbyDemo/Utilities/PoseInterpolator.swift | 27 + NearbyDemoTests/NearbyDemoTests.swift | 19 + NearbyDemoUITests/NearbyDemoUITests.swift | 43 ++ .../NearbyDemoUITestsLaunchTests.swift | 35 + 21 files changed, 1860 insertions(+) create mode 100644 .DS_Store create mode 100644 NearbyDemo.xcodeproj/project.pbxproj create mode 100644 NearbyDemo.xcodeproj/project.xcworkspace/contents.xcworkspacedata create mode 100644 NearbyDemo/ARViewContainer.swift create mode 100644 NearbyDemo/Assets.xcassets/AccentColor.colorset/Contents.json create mode 100644 NearbyDemo/Assets.xcassets/AppIcon.appiconset/Contents.json create mode 100644 NearbyDemo/Assets.xcassets/Contents.json create mode 100644 NearbyDemo/ContentView.swift create mode 100644 NearbyDemo/Info.plist create mode 100644 NearbyDemo/Managers/ARManager.swift create mode 100644 NearbyDemo/Managers/AnchorEstimator.swift create mode 100644 NearbyDemo/Managers/BLEManager.swift create mode 100644 NearbyDemo/Managers/NIManager.swift create mode 100644 NearbyDemo/Models/QorvoBLEUUIDs.swift create mode 100644 NearbyDemo/Models/TimestampedPose.swift create mode 100644 NearbyDemo/Models/TimestampedRange.swift create mode 100644 NearbyDemo/NearbyDemoApp.swift create mode 100644 NearbyDemo/Utilities/PoseInterpolator.swift create mode 100644 NearbyDemoTests/NearbyDemoTests.swift create mode 100644 NearbyDemoUITests/NearbyDemoUITests.swift create mode 100644 NearbyDemoUITests/NearbyDemoUITestsLaunchTests.swift diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..962675a649042424dccebd81deb3e0b74d12ed49 GIT binary patch literal 8196 zcmeI1y-EW?6ov1^Ac+<#q)547A4sVntYIwzViT>jh)MiG5;hnG?bhBm5TC#Y@C{-t z?DPSA0vkIEJ$Gg_yJTZFf)el!%$#NSp85Hb-O1e$kt&@wibQ!L%Am5&&!UMbe4Xn+ zN!c?wsDhuUM(5RJtq>PH6}odQ79IIRngkq2mxW7V}97}ZwvX|e}pt;(ht%Ghz;;c%#HH85)Iq>P=E z9a-57Md{Jub96W<*Qm5YKnM&H;M{$NHjuOL(s4L{FE{;qt?Acroo}gi)WW+iI2z(< z9uzBeztz?eG~FE**YZ8j_MO>TUU|N}zgrz1+ejSSJ`9gvrLEGl)=-*ICBVd=`P;(t_+vNW4HW1tDpEI{LF)PE|%XWxOM1+Ix*C4 zLexgMqpZ|5SdDOm2|c7Tk1sv_%^B(Hb4l)s&@kK5LM)9fD${pF-a_N^ zt}vPX*X$>mM9w~)@tjdY4kpUK6=eTs5K@G|gcF!D^UHJozlQr(-UCed2PV@K0>3~& zrM-$*LdWJO-{1(GYdfe_R4yDB7}XXuIvt1VbR7Eiharv~gtEuA8W_bH6nPP#F-Ri> H{*=H6pScUu literal 0 HcmV?d00001 diff --git a/NearbyDemo.xcodeproj/project.pbxproj b/NearbyDemo.xcodeproj/project.pbxproj new file mode 100644 index 0000000..cecb0e8 --- /dev/null +++ b/NearbyDemo.xcodeproj/project.pbxproj @@ -0,0 +1,631 @@ +// !$*UTF8*$! +{ + archiveVersion = 1; + classes = { + }; + objectVersion = 77; + objects = { + +/* Begin PBXBuildFile section */ + 86A1B8E92F931912007D8DF7 /* NearbyInteraction.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A1B8E82F931912007D8DF7 /* NearbyInteraction.framework */; }; + 86A1B8EB2F931918007D8DF7 /* CoreBluetooth.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A1B8EA2F931918007D8DF7 /* CoreBluetooth.framework */; }; + 86A1B8ED2F931920007D8DF7 /* ARKit.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A1B8EC2F931920007D8DF7 /* ARKit.framework */; }; + 86A1B8EF2F93192A007D8DF7 /* RealityKit.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A1B8EE2F93192A007D8DF7 /* RealityKit.framework */; }; +/* End PBXBuildFile section */ + +/* Begin PBXContainerItemProxy section */ + 86D2C31C2F91DFA80031AF9B /* PBXContainerItemProxy */ = { + isa = PBXContainerItemProxy; + containerPortal = 86D2C3062F91DFA50031AF9B /* Project object */; + proxyType = 1; + remoteGlobalIDString = 86D2C30D2F91DFA50031AF9B; + remoteInfo = NearbyDemo; + }; + 86D2C3262F91DFA80031AF9B /* PBXContainerItemProxy */ = { + isa = PBXContainerItemProxy; + containerPortal = 86D2C3062F91DFA50031AF9B /* Project object */; + proxyType = 1; + remoteGlobalIDString = 86D2C30D2F91DFA50031AF9B; + remoteInfo = NearbyDemo; + }; +/* End PBXContainerItemProxy section */ + +/* Begin PBXFileReference section */ + 86A1B8E82F931912007D8DF7 /* NearbyInteraction.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = NearbyInteraction.framework; path = System/Library/Frameworks/NearbyInteraction.framework; sourceTree = SDKROOT; }; + 86A1B8EA2F931918007D8DF7 /* CoreBluetooth.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreBluetooth.framework; path = System/Library/Frameworks/CoreBluetooth.framework; sourceTree = SDKROOT; }; + 86A1B8EC2F931920007D8DF7 /* ARKit.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = ARKit.framework; path = System/Library/Frameworks/ARKit.framework; sourceTree = SDKROOT; }; + 86A1B8EE2F93192A007D8DF7 /* RealityKit.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = RealityKit.framework; path = System/Library/Frameworks/RealityKit.framework; sourceTree = SDKROOT; }; + 86D2C30E2F91DFA50031AF9B /* NearbyDemo.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = NearbyDemo.app; sourceTree = BUILT_PRODUCTS_DIR; }; + 86D2C31B2F91DFA80031AF9B /* NearbyDemoTests.xctest */ = {isa = PBXFileReference; explicitFileType = wrapper.cfbundle; includeInIndex = 0; path = NearbyDemoTests.xctest; sourceTree = BUILT_PRODUCTS_DIR; }; + 86D2C3252F91DFA80031AF9B /* NearbyDemoUITests.xctest */ = {isa = PBXFileReference; explicitFileType = wrapper.cfbundle; includeInIndex = 0; path = NearbyDemoUITests.xctest; sourceTree = BUILT_PRODUCTS_DIR; }; +/* End PBXFileReference section */ + +/* Begin PBXFileSystemSynchronizedBuildFileExceptionSet section */ + 86A1B8F12F9319C3007D8DF7 /* Exceptions for "NearbyDemo" folder in "NearbyDemo" target */ = { + isa = PBXFileSystemSynchronizedBuildFileExceptionSet; + membershipExceptions = ( + Info.plist, + ); + target = 86D2C30D2F91DFA50031AF9B /* NearbyDemo */; + }; +/* End PBXFileSystemSynchronizedBuildFileExceptionSet section */ + +/* Begin PBXFileSystemSynchronizedRootGroup section */ + 86D2C3102F91DFA50031AF9B /* NearbyDemo */ = { + isa = PBXFileSystemSynchronizedRootGroup; + exceptions = ( + 86A1B8F12F9319C3007D8DF7 /* Exceptions for "NearbyDemo" folder in "NearbyDemo" target */, + ); + path = NearbyDemo; + sourceTree = ""; + }; + 86D2C31E2F91DFA80031AF9B /* NearbyDemoTests */ = { + isa = PBXFileSystemSynchronizedRootGroup; + path = NearbyDemoTests; + sourceTree = ""; + }; + 86D2C3282F91DFA80031AF9B /* NearbyDemoUITests */ = { + isa = PBXFileSystemSynchronizedRootGroup; + path = NearbyDemoUITests; + sourceTree = ""; + }; +/* End PBXFileSystemSynchronizedRootGroup section */ + +/* Begin PBXFrameworksBuildPhase section */ + 86D2C30B2F91DFA50031AF9B /* Frameworks */ = { + isa = PBXFrameworksBuildPhase; + buildActionMask = 2147483647; + files = ( + 86A1B8ED2F931920007D8DF7 /* ARKit.framework in Frameworks */, + 86A1B8EB2F931918007D8DF7 /* CoreBluetooth.framework in Frameworks */, + 86A1B8EF2F93192A007D8DF7 /* RealityKit.framework in Frameworks */, + 86A1B8E92F931912007D8DF7 /* NearbyInteraction.framework in Frameworks */, + ); + runOnlyForDeploymentPostprocessing = 0; + }; + 86D2C3182F91DFA80031AF9B /* Frameworks */ = { + isa = PBXFrameworksBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; + 86D2C3222F91DFA80031AF9B /* Frameworks */ = { + isa = PBXFrameworksBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXFrameworksBuildPhase section */ + +/* Begin PBXGroup section */ + 86A1B8E72F931911007D8DF7 /* Frameworks */ = { + isa = PBXGroup; + children = ( + 86A1B8EE2F93192A007D8DF7 /* RealityKit.framework */, + 86A1B8EC2F931920007D8DF7 /* ARKit.framework */, + 86A1B8EA2F931918007D8DF7 /* CoreBluetooth.framework */, + 86A1B8E82F931912007D8DF7 /* NearbyInteraction.framework */, + ); + name = Frameworks; + sourceTree = ""; + }; + 86D2C3052F91DFA50031AF9B = { + isa = PBXGroup; + children = ( + 86D2C3102F91DFA50031AF9B /* NearbyDemo */, + 86D2C31E2F91DFA80031AF9B /* NearbyDemoTests */, + 86D2C3282F91DFA80031AF9B /* NearbyDemoUITests */, + 86A1B8E72F931911007D8DF7 /* Frameworks */, + 86D2C30F2F91DFA50031AF9B /* Products */, + ); + sourceTree = ""; + }; + 86D2C30F2F91DFA50031AF9B /* Products */ = { + isa = PBXGroup; + children = ( + 86D2C30E2F91DFA50031AF9B /* NearbyDemo.app */, + 86D2C31B2F91DFA80031AF9B /* NearbyDemoTests.xctest */, + 86D2C3252F91DFA80031AF9B /* NearbyDemoUITests.xctest */, + ); + name = Products; + sourceTree = ""; + }; +/* End PBXGroup section */ + +/* Begin PBXNativeTarget section */ + 86D2C30D2F91DFA50031AF9B /* NearbyDemo */ = { + isa = PBXNativeTarget; + buildConfigurationList = 86D2C32F2F91DFA80031AF9B /* Build configuration list for PBXNativeTarget "NearbyDemo" */; + buildPhases = ( + 86D2C30A2F91DFA50031AF9B /* Sources */, + 86D2C30B2F91DFA50031AF9B /* Frameworks */, + 86D2C30C2F91DFA50031AF9B /* Resources */, + ); + buildRules = ( + ); + dependencies = ( + ); + fileSystemSynchronizedGroups = ( + 86D2C3102F91DFA50031AF9B /* NearbyDemo */, + ); + name = NearbyDemo; + packageProductDependencies = ( + ); + productName = NearbyDemo; + productReference = 86D2C30E2F91DFA50031AF9B /* NearbyDemo.app */; + productType = "com.apple.product-type.application"; + }; + 86D2C31A2F91DFA80031AF9B /* NearbyDemoTests */ = { + isa = PBXNativeTarget; + buildConfigurationList = 86D2C3322F91DFA80031AF9B /* Build configuration list for PBXNativeTarget "NearbyDemoTests" */; + buildPhases = ( + 86D2C3172F91DFA80031AF9B /* Sources */, + 86D2C3182F91DFA80031AF9B /* Frameworks */, + 86D2C3192F91DFA80031AF9B /* Resources */, + ); + buildRules = ( + ); + dependencies = ( + 86D2C31D2F91DFA80031AF9B /* PBXTargetDependency */, + ); + fileSystemSynchronizedGroups = ( + 86D2C31E2F91DFA80031AF9B /* NearbyDemoTests */, + ); + name = NearbyDemoTests; + packageProductDependencies = ( + ); + productName = NearbyDemoTests; + productReference = 86D2C31B2F91DFA80031AF9B /* NearbyDemoTests.xctest */; + productType = "com.apple.product-type.bundle.unit-test"; + }; + 86D2C3242F91DFA80031AF9B /* NearbyDemoUITests */ = { + isa = PBXNativeTarget; + buildConfigurationList = 86D2C3352F91DFA80031AF9B /* Build configuration list for PBXNativeTarget "NearbyDemoUITests" */; + buildPhases = ( + 86D2C3212F91DFA80031AF9B /* Sources */, + 86D2C3222F91DFA80031AF9B /* Frameworks */, + 86D2C3232F91DFA80031AF9B /* Resources */, + ); + buildRules = ( + ); + dependencies = ( + 86D2C3272F91DFA80031AF9B /* PBXTargetDependency */, + ); + fileSystemSynchronizedGroups = ( + 86D2C3282F91DFA80031AF9B /* NearbyDemoUITests */, + ); + name = NearbyDemoUITests; + packageProductDependencies = ( + ); + productName = NearbyDemoUITests; + productReference = 86D2C3252F91DFA80031AF9B /* NearbyDemoUITests.xctest */; + productType = "com.apple.product-type.bundle.ui-testing"; + }; +/* End PBXNativeTarget section */ + +/* Begin PBXProject section */ + 86D2C3062F91DFA50031AF9B /* Project object */ = { + isa = PBXProject; + attributes = { + BuildIndependentTargetsInParallel = 1; + LastSwiftUpdateCheck = 2640; + LastUpgradeCheck = 2640; + TargetAttributes = { + 86D2C30D2F91DFA50031AF9B = { + CreatedOnToolsVersion = 26.4; + }; + 86D2C31A2F91DFA80031AF9B = { + CreatedOnToolsVersion = 26.4; + TestTargetID = 86D2C30D2F91DFA50031AF9B; + }; + 86D2C3242F91DFA80031AF9B = { + CreatedOnToolsVersion = 26.4; + TestTargetID = 86D2C30D2F91DFA50031AF9B; + }; + }; + }; + buildConfigurationList = 86D2C3092F91DFA50031AF9B /* Build configuration list for PBXProject "NearbyDemo" */; + developmentRegion = en; + hasScannedForEncodings = 0; + knownRegions = ( + en, + Base, + ); + mainGroup = 86D2C3052F91DFA50031AF9B; + minimizedProjectReferenceProxies = 1; + preferredProjectObjectVersion = 77; + productRefGroup = 86D2C30F2F91DFA50031AF9B /* Products */; + projectDirPath = ""; + projectRoot = ""; + targets = ( + 86D2C30D2F91DFA50031AF9B /* NearbyDemo */, + 86D2C31A2F91DFA80031AF9B /* NearbyDemoTests */, + 86D2C3242F91DFA80031AF9B /* NearbyDemoUITests */, + ); + }; +/* End PBXProject section */ + +/* Begin PBXResourcesBuildPhase section */ + 86D2C30C2F91DFA50031AF9B /* Resources */ = { + isa = PBXResourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; + 86D2C3192F91DFA80031AF9B /* Resources */ = { + isa = PBXResourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; + 86D2C3232F91DFA80031AF9B /* Resources */ = { + isa = PBXResourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXResourcesBuildPhase section */ + +/* Begin PBXSourcesBuildPhase section */ + 86D2C30A2F91DFA50031AF9B /* Sources */ = { + isa = PBXSourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; + 86D2C3172F91DFA80031AF9B /* Sources */ = { + isa = PBXSourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; + 86D2C3212F91DFA80031AF9B /* Sources */ = { + isa = PBXSourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXSourcesBuildPhase section */ + +/* Begin PBXTargetDependency section */ + 86D2C31D2F91DFA80031AF9B /* PBXTargetDependency */ = { + isa = PBXTargetDependency; + target = 86D2C30D2F91DFA50031AF9B /* NearbyDemo */; + targetProxy = 86D2C31C2F91DFA80031AF9B /* PBXContainerItemProxy */; + }; + 86D2C3272F91DFA80031AF9B /* PBXTargetDependency */ = { + isa = PBXTargetDependency; + target = 86D2C30D2F91DFA50031AF9B /* NearbyDemo */; + targetProxy = 86D2C3262F91DFA80031AF9B /* PBXContainerItemProxy */; + }; +/* End PBXTargetDependency section */ + +/* Begin XCBuildConfiguration section */ + 86D2C32D2F91DFA80031AF9B /* 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_SELF = 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; + DEVELOPMENT_TEAM = YK2DB9NT3S; + 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 = 26.4; + LOCALIZATION_PREFERS_STRING_CATALOGS = YES; + MTL_ENABLE_DEBUG_INFO = INCLUDE_SOURCE; + MTL_FAST_MATH = YES; + ONLY_ACTIVE_ARCH = YES; + SDKROOT = iphoneos; + SWIFT_ACTIVE_COMPILATION_CONDITIONS = "DEBUG $(inherited)"; + SWIFT_OPTIMIZATION_LEVEL = "-Onone"; + }; + name = Debug; + }; + 86D2C32E2F91DFA80031AF9B /* 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_SELF = 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"; + DEVELOPMENT_TEAM = YK2DB9NT3S; + 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 = 26.4; + LOCALIZATION_PREFERS_STRING_CATALOGS = YES; + MTL_ENABLE_DEBUG_INFO = NO; + MTL_FAST_MATH = YES; + SDKROOT = iphoneos; + SWIFT_COMPILATION_MODE = wholemodule; + VALIDATE_PRODUCT = YES; + }; + name = Release; + }; + 86D2C3302F91DFA80031AF9B /* 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_TEAM = YK2DB9NT3S; + ENABLE_PREVIEWS = YES; + GENERATE_INFOPLIST_FILE = YES; + INFOPLIST_FILE = NearbyDemo/Info.plist; + INFOPLIST_KEY_NSBluetoothAlwaysUsageDescription = "Bluetooth necessary for token exchange with DWM module"; + INFOPLIST_KEY_NSCameraUsageDescription = "Camera necessary for local environment tracking and AR experience."; + INFOPLIST_KEY_NSNearbyInteractionUsageDescription = "Nearby interaction necessary to place user in space relative to DWM module."; + INFOPLIST_KEY_UIApplicationSceneManifest_Generation = YES; + INFOPLIST_KEY_UIApplicationSupportsIndirectInputEvents = YES; + INFOPLIST_KEY_UILaunchScreen_Generation = YES; + INFOPLIST_KEY_UISupportedInterfaceOrientations_iPad = "UIInterfaceOrientationPortrait UIInterfaceOrientationPortraitUpsideDown UIInterfaceOrientationLandscapeLeft UIInterfaceOrientationLandscapeRight"; + INFOPLIST_KEY_UISupportedInterfaceOrientations_iPhone = "UIInterfaceOrientationPortrait UIInterfaceOrientationLandscapeLeft UIInterfaceOrientationLandscapeRight"; + LD_RUNPATH_SEARCH_PATHS = ( + "$(inherited)", + "@executable_path/Frameworks", + ); + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.adipu.NearbyDemo; + PRODUCT_NAME = "$(TARGET_NAME)"; + STRING_CATALOG_GENERATE_SYMBOLS = YES; + SWIFT_APPROACHABLE_CONCURRENCY = YES; + SWIFT_DEFAULT_ACTOR_ISOLATION = MainActor; + SWIFT_EMIT_LOC_STRINGS = YES; + SWIFT_UPCOMING_FEATURE_MEMBER_IMPORT_VISIBILITY = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = "1,2"; + }; + name = Debug; + }; + 86D2C3312F91DFA80031AF9B /* 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_TEAM = YK2DB9NT3S; + ENABLE_PREVIEWS = YES; + GENERATE_INFOPLIST_FILE = YES; + INFOPLIST_FILE = NearbyDemo/Info.plist; + INFOPLIST_KEY_NSBluetoothAlwaysUsageDescription = "Bluetooth necessary for token exchange with DWM module"; + INFOPLIST_KEY_NSCameraUsageDescription = "Camera necessary for local environment tracking and AR experience."; + INFOPLIST_KEY_NSNearbyInteractionUsageDescription = "Nearby interaction necessary to place user in space relative to DWM module."; + INFOPLIST_KEY_UIApplicationSceneManifest_Generation = YES; + INFOPLIST_KEY_UIApplicationSupportsIndirectInputEvents = YES; + INFOPLIST_KEY_UILaunchScreen_Generation = YES; + INFOPLIST_KEY_UISupportedInterfaceOrientations_iPad = "UIInterfaceOrientationPortrait UIInterfaceOrientationPortraitUpsideDown UIInterfaceOrientationLandscapeLeft UIInterfaceOrientationLandscapeRight"; + INFOPLIST_KEY_UISupportedInterfaceOrientations_iPhone = "UIInterfaceOrientationPortrait UIInterfaceOrientationLandscapeLeft UIInterfaceOrientationLandscapeRight"; + LD_RUNPATH_SEARCH_PATHS = ( + "$(inherited)", + "@executable_path/Frameworks", + ); + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.adipu.NearbyDemo; + PRODUCT_NAME = "$(TARGET_NAME)"; + STRING_CATALOG_GENERATE_SYMBOLS = YES; + SWIFT_APPROACHABLE_CONCURRENCY = YES; + SWIFT_DEFAULT_ACTOR_ISOLATION = MainActor; + SWIFT_EMIT_LOC_STRINGS = YES; + SWIFT_UPCOMING_FEATURE_MEMBER_IMPORT_VISIBILITY = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = "1,2"; + }; + name = Release; + }; + 86D2C3332F91DFA80031AF9B /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + BUNDLE_LOADER = "$(TEST_HOST)"; + CODE_SIGN_STYLE = Automatic; + CURRENT_PROJECT_VERSION = 1; + DEVELOPMENT_TEAM = YK2DB9NT3S; + GENERATE_INFOPLIST_FILE = YES; + IPHONEOS_DEPLOYMENT_TARGET = 26.4; + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.adipu.NearbyDemoTests; + PRODUCT_NAME = "$(TARGET_NAME)"; + STRING_CATALOG_GENERATE_SYMBOLS = NO; + SWIFT_APPROACHABLE_CONCURRENCY = YES; + SWIFT_EMIT_LOC_STRINGS = NO; + SWIFT_UPCOMING_FEATURE_MEMBER_IMPORT_VISIBILITY = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = "1,2"; + TEST_HOST = "$(BUILT_PRODUCTS_DIR)/NearbyDemo.app/$(BUNDLE_EXECUTABLE_FOLDER_PATH)/NearbyDemo"; + }; + name = Debug; + }; + 86D2C3342F91DFA80031AF9B /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + BUNDLE_LOADER = "$(TEST_HOST)"; + CODE_SIGN_STYLE = Automatic; + CURRENT_PROJECT_VERSION = 1; + DEVELOPMENT_TEAM = YK2DB9NT3S; + GENERATE_INFOPLIST_FILE = YES; + IPHONEOS_DEPLOYMENT_TARGET = 26.4; + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.adipu.NearbyDemoTests; + PRODUCT_NAME = "$(TARGET_NAME)"; + STRING_CATALOG_GENERATE_SYMBOLS = NO; + SWIFT_APPROACHABLE_CONCURRENCY = YES; + SWIFT_EMIT_LOC_STRINGS = NO; + SWIFT_UPCOMING_FEATURE_MEMBER_IMPORT_VISIBILITY = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = "1,2"; + TEST_HOST = "$(BUILT_PRODUCTS_DIR)/NearbyDemo.app/$(BUNDLE_EXECUTABLE_FOLDER_PATH)/NearbyDemo"; + }; + name = Release; + }; + 86D2C3362F91DFA80031AF9B /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + CODE_SIGN_STYLE = Automatic; + CURRENT_PROJECT_VERSION = 1; + DEVELOPMENT_TEAM = YK2DB9NT3S; + GENERATE_INFOPLIST_FILE = YES; + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.adipu.NearbyDemoUITests; + PRODUCT_NAME = "$(TARGET_NAME)"; + STRING_CATALOG_GENERATE_SYMBOLS = NO; + SWIFT_APPROACHABLE_CONCURRENCY = YES; + SWIFT_EMIT_LOC_STRINGS = NO; + SWIFT_UPCOMING_FEATURE_MEMBER_IMPORT_VISIBILITY = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = "1,2"; + TEST_TARGET_NAME = NearbyDemo; + }; + name = Debug; + }; + 86D2C3372F91DFA80031AF9B /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + CODE_SIGN_STYLE = Automatic; + CURRENT_PROJECT_VERSION = 1; + DEVELOPMENT_TEAM = YK2DB9NT3S; + GENERATE_INFOPLIST_FILE = YES; + MARKETING_VERSION = 1.0; + PRODUCT_BUNDLE_IDENTIFIER = com.adipu.NearbyDemoUITests; + PRODUCT_NAME = "$(TARGET_NAME)"; + STRING_CATALOG_GENERATE_SYMBOLS = NO; + SWIFT_APPROACHABLE_CONCURRENCY = YES; + SWIFT_EMIT_LOC_STRINGS = NO; + SWIFT_UPCOMING_FEATURE_MEMBER_IMPORT_VISIBILITY = YES; + SWIFT_VERSION = 5.0; + TARGETED_DEVICE_FAMILY = "1,2"; + TEST_TARGET_NAME = NearbyDemo; + }; + name = Release; + }; +/* End XCBuildConfiguration section */ + +/* Begin XCConfigurationList section */ + 86D2C3092F91DFA50031AF9B /* Build configuration list for PBXProject "NearbyDemo" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 86D2C32D2F91DFA80031AF9B /* Debug */, + 86D2C32E2F91DFA80031AF9B /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; + 86D2C32F2F91DFA80031AF9B /* Build configuration list for PBXNativeTarget "NearbyDemo" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 86D2C3302F91DFA80031AF9B /* Debug */, + 86D2C3312F91DFA80031AF9B /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; + 86D2C3322F91DFA80031AF9B /* Build configuration list for PBXNativeTarget "NearbyDemoTests" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 86D2C3332F91DFA80031AF9B /* Debug */, + 86D2C3342F91DFA80031AF9B /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; + 86D2C3352F91DFA80031AF9B /* Build configuration list for PBXNativeTarget "NearbyDemoUITests" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 86D2C3362F91DFA80031AF9B /* Debug */, + 86D2C3372F91DFA80031AF9B /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; +/* End XCConfigurationList section */ + }; + rootObject = 86D2C3062F91DFA50031AF9B /* Project object */; +} diff --git a/NearbyDemo.xcodeproj/project.xcworkspace/contents.xcworkspacedata b/NearbyDemo.xcodeproj/project.xcworkspace/contents.xcworkspacedata new file mode 100644 index 0000000..919434a --- /dev/null +++ b/NearbyDemo.xcodeproj/project.xcworkspace/contents.xcworkspacedata @@ -0,0 +1,7 @@ + + + + + diff --git a/NearbyDemo/ARViewContainer.swift b/NearbyDemo/ARViewContainer.swift new file mode 100644 index 0000000..aa5f862 --- /dev/null +++ b/NearbyDemo/ARViewContainer.swift @@ -0,0 +1,104 @@ +import SwiftUI +import RealityKit +import ARKit +import Combine + +struct ARViewContainer: UIViewRepresentable { + @ObservedObject var arManager: ARManager + @ObservedObject var estimator: AnchorEstimator + + func makeUIView(context: Context) -> ARView { + let arView = ARView(frame: .zero, cameraMode: .ar, automaticallyConfigureSession: false) + arView.session = arManager.session + + // Create the red sphere entity representing the UWB anchor + let sphereRadius: Float = 0.05 + let mesh = MeshResource.generateSphere(radius: sphereRadius) + var material = UnlitMaterial() + material.color = .init(tint: .red) + let sphereEntity = ModelEntity(mesh: mesh, materials: [material]) + sphereEntity.isEnabled = false + + let anchorEntity = AnchorEntity(world: .zero) + anchorEntity.addChild(sphereEntity) + arView.scene.addAnchor(anchorEntity) + + let coordinator = context.coordinator + coordinator.sphereEntity = sphereEntity + + // SceneEvents.Update fires every rendered frame on the main thread. + // Updating sphere position here (rather than from the Combine sink) means: + // 1. The sphere is never "frozen" between 4 Hz UWB pings — it always reflects + // the latest world-space estimate relative to the smoothly moving camera. + // 2. We can apply EMA smoothing to hide step-changes from the solver. + coordinator.updateSub = arView.scene.subscribe(to: SceneEvents.Update.self) { [weak estimator, weak coordinator, weak sphereEntity, weak arView] _ in + guard let estimator, let coordinator, let sphereEntity else { return } + + // EMA-smooth toward the latest estimate every frame. + // α ≈ 0.12 at 60 fps → time constant ≈ 120 ms. Hides solver step-changes + // without making the sphere feel sluggish. + let alpha: Float = 0.12 + if let target = estimator.anchorPosition { + if let current = coordinator.smoothedPosition { + coordinator.smoothedPosition = current + alpha * (target - current) + } else { + coordinator.smoothedPosition = target // snap on first appearance + } + sphereEntity.setPosition(coordinator.smoothedPosition!, relativeTo: nil) + sphereEntity.isEnabled = true + } else { + coordinator.smoothedPosition = nil + sphereEntity.isEnabled = false + } + + // Off-screen directional indicator using the smoothed position + guard let arView else { return } + guard let position = coordinator.smoothedPosition else { + if coordinator.lastAngle != nil { + DispatchQueue.main.async { estimator.offScreenAngle = nil } + coordinator.lastAngle = nil + } + return + } + + let isOffScreen: Bool + if let proj = arView.project(position) { + isOffScreen = !arView.bounds.contains(proj) + } else { + isOffScreen = true + } + + if isOffScreen { + guard let camera = arView.session.currentFrame?.camera else { return } + let cameraTransform = camera.transform + let localPos4 = simd_mul(simd_inverse(cameraTransform), + simd_float4(position.x, position.y, position.z, 1.0)) + let angle = Double(atan2(localPos4.y, localPos4.x)) + if coordinator.lastAngle == nil || abs(coordinator.lastAngle! - angle) > 0.05 { + coordinator.lastAngle = angle + DispatchQueue.main.async { estimator.offScreenAngle = angle } + } + } else { + if coordinator.lastAngle != nil { + DispatchQueue.main.async { estimator.offScreenAngle = nil } + coordinator.lastAngle = nil + } + } + } + + return arView + } + + func updateUIView(_ uiView: ARView, context: Context) {} + + func makeCoordinator() -> Coordinator { + Coordinator() + } + + class Coordinator { + var sphereEntity: ModelEntity? + var updateSub: (any Cancellable)? + var smoothedPosition: simd_float3? = nil + var lastAngle: Double? + } +} diff --git a/NearbyDemo/Assets.xcassets/AccentColor.colorset/Contents.json b/NearbyDemo/Assets.xcassets/AccentColor.colorset/Contents.json new file mode 100644 index 0000000..eb87897 --- /dev/null +++ b/NearbyDemo/Assets.xcassets/AccentColor.colorset/Contents.json @@ -0,0 +1,11 @@ +{ + "colors" : [ + { + "idiom" : "universal" + } + ], + "info" : { + "author" : "xcode", + "version" : 1 + } +} diff --git a/NearbyDemo/Assets.xcassets/AppIcon.appiconset/Contents.json b/NearbyDemo/Assets.xcassets/AppIcon.appiconset/Contents.json new file mode 100644 index 0000000..2305880 --- /dev/null +++ b/NearbyDemo/Assets.xcassets/AppIcon.appiconset/Contents.json @@ -0,0 +1,35 @@ +{ + "images" : [ + { + "idiom" : "universal", + "platform" : "ios", + "size" : "1024x1024" + }, + { + "appearances" : [ + { + "appearance" : "luminosity", + "value" : "dark" + } + ], + "idiom" : "universal", + "platform" : "ios", + "size" : "1024x1024" + }, + { + "appearances" : [ + { + "appearance" : "luminosity", + "value" : "tinted" + } + ], + "idiom" : "universal", + "platform" : "ios", + "size" : "1024x1024" + } + ], + "info" : { + "author" : "xcode", + "version" : 1 + } +} diff --git a/NearbyDemo/Assets.xcassets/Contents.json b/NearbyDemo/Assets.xcassets/Contents.json new file mode 100644 index 0000000..73c0059 --- /dev/null +++ b/NearbyDemo/Assets.xcassets/Contents.json @@ -0,0 +1,6 @@ +{ + "info" : { + "author" : "xcode", + "version" : 1 + } +} diff --git a/NearbyDemo/ContentView.swift b/NearbyDemo/ContentView.swift new file mode 100644 index 0000000..ece10bd --- /dev/null +++ b/NearbyDemo/ContentView.swift @@ -0,0 +1,117 @@ +import SwiftUI +import ARKit + +struct ContentView: View { + @EnvironmentObject var ble: BLEManager + @EnvironmentObject var ni: NIManager + @EnvironmentObject var ar: ARManager + @EnvironmentObject var estimator: AnchorEstimator + + var body: some View { + ZStack { + ARViewContainer(arManager: ar, estimator: estimator) + .ignoresSafeArea() + + if let angle = estimator.offScreenAngle { + Image(systemName: "location.north.fill") + .resizable() + .aspectRatio(contentMode: .fit) + .frame(width: 40, height: 40) + .foregroundColor(.red) + .shadow(radius: 5) + // The image points "Up". + // SwiftUI rotates clockwise. Angle is mathematically CCW. + // If angle = 0 (right), we need it to point Right, so rotate 90 deg clockwise. + // Actually, if image points UP, we rotate by PI/2 - angle. + .rotationEffect(.radians(.pi / 2 - angle)) + // Offset moves it towards the edge + .offset(x: cos(angle) * 140, y: -sin(angle) * 140) + .animation(.interactiveSpring(), value: angle) + } + + VStack { + HUDView(ble: ble, ni: ni, estimator: estimator) + .padding() + Spacer() + Button("Reset Estimate") { + estimator.reset() + } + .buttonStyle(.borderedProminent) + .tint(.red.opacity(0.8)) + .padding(.bottom, 40) + } + } + } +} + +// MARK: - HUD overlay + +private struct HUDView: View { + @ObservedObject var ble: BLEManager + @ObservedObject var ni: NIManager + @ObservedObject var estimator: AnchorEstimator + + var body: some View { + VStack(alignment: .leading, spacing: 6) { + HUDRow(label: "BLE", value: bleStateText) + HUDRow(label: "NI", value: niStateText) + HUDRow(label: "Range", value: rangeText) + HUDRow(label: "Measurements", value: "\(estimator.measurementCount)") + HUDRow(label: "Residual", value: String(format: "%.3f m", estimator.residualError)) + } + .padding(12) + .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 12)) + .frame(maxWidth: .infinity, alignment: .leading) + } + + private var bleStateText: String { + switch ble.connectionState { + case .disconnected: return "Disconnected" + case .scanning: return "Scanning…" + case .connecting: return "Connecting…" + case .connected: return "Connected" + } + } + + private var niStateText: String { + switch ni.sessionState { + case .idle: return "Idle" + case .waitingForAccessory: return "Waiting for board…" + case .configuring: return "Configuring…" + case .ranging: return "Ranging" + case .error(let msg): return "Error: \(msg)" + } + } + + private var rangeText: String { + if let r = ni.lastRange { + return String(format: "%.2f m", r) + } + return "—" + } +} + +private struct HUDRow: View { + let label: String + let value: String + + var body: some View { + HStack { + Text(label) + .font(.caption.bold()) + .foregroundStyle(.secondary) + .frame(width: 100, alignment: .leading) + Text(value) + .font(.caption) + .foregroundStyle(.primary) + } + } +} + +#Preview { + ContentView() + .environmentObject(BLEManager()) + .environmentObject(NIManager()) + .environmentObject(ARManager()) + .environmentObject(AnchorEstimator()) +} diff --git a/NearbyDemo/Info.plist b/NearbyDemo/Info.plist new file mode 100644 index 0000000..40692d0 --- /dev/null +++ b/NearbyDemo/Info.plist @@ -0,0 +1,17 @@ + + + + + UIBackgroundModes + + bluetooth-central + nearby-interaction + + NSCameraUsageDescription + Camera is required for ARKit world tracking to estimate the UWB anchor position in 3D space. + NSBluetoothAlwaysUsageDescription + Bluetooth is used to communicate with the Qorvo DWM3001CDK UWB anchor and exchange NearbyInteraction session configuration data. + NSNearbyInteractionUsageDescription + Nearby Interaction is used to measure precise UWB distances to the DWM3001CDK anchor device. + + diff --git a/NearbyDemo/Managers/ARManager.swift b/NearbyDemo/Managers/ARManager.swift new file mode 100644 index 0000000..2fb1dae --- /dev/null +++ b/NearbyDemo/Managers/ARManager.swift @@ -0,0 +1,92 @@ +import ARKit +import Combine +import Foundation +import simd + +class ARManager: NSObject, ObservableObject { + let session = ARSession() + + @Published var trackingState: ARCamera.TrackingState = .notAvailable + + private(set) var poseBuffer: [TimestampedPose] = [] + private let bufferCapacity = 120 + private let bufferLock = NSLock() + + override init() { + super.init() + session.delegate = self + } + + func start() { + let config = ARWorldTrackingConfiguration() + config.worldAlignment = .gravity + config.planeDetection = [] + config.isAutoFocusEnabled = false + session.run(config, options: [.resetTracking, .removeExistingAnchors]) + print("[AR] Session started") + } + + func stop() { + session.pause() + print("[AR] Session paused") + } + + /// Returns the interpolated camera pose at the given system-uptime timestamp, + /// or nil if the timestamp is outside the current buffer range. + func poseAt(timestamp: TimeInterval) -> simd_float4x4? { + bufferLock.lock() + let buffer = poseBuffer + bufferLock.unlock() + + guard buffer.count >= 2 else { return buffer.first?.transform } + + // Outside range — clamp to edges + if timestamp <= buffer.first!.timestamp { return buffer.first?.transform } + if timestamp >= buffer.last!.timestamp { return buffer.last?.transform } + + // Binary search for the bracketing pair + var lo = 0, hi = buffer.count - 1 + while lo + 1 < hi { + let mid = (lo + hi) / 2 + if buffer[mid].timestamp <= timestamp { lo = mid } else { hi = mid } + } + return PoseInterpolator.interpolate(from: buffer[lo], to: buffer[hi], at: timestamp) + } +} + +// MARK: - ARSessionDelegate +extension ARManager: ARSessionDelegate { + func session(_ session: ARSession, didUpdate frame: ARFrame) { + let pose = TimestampedPose(transform: frame.camera.transform, timestamp: frame.timestamp) + + bufferLock.lock() + poseBuffer.append(pose) + if poseBuffer.count > bufferCapacity { + poseBuffer.removeFirst() + } + bufferLock.unlock() + + let state = frame.camera.trackingState + DispatchQueue.main.async { + self.trackingState = state + } + } + + func sessionShouldAttemptRelocalization(_ session: ARSession) -> Bool { + // NI camera assistance requires this to return false + return false + } + + func session(_ session: ARSession, didFailWithError error: Error) { + print("[AR] Session failed: \(error)") + } + + func sessionWasInterrupted(_ session: ARSession) { + print("[AR] Session interrupted") + } + + func sessionInterruptionEnded(_ session: ARSession) { + print("[AR] Session interruption ended — resetting") + start() + } +} diff --git a/NearbyDemo/Managers/AnchorEstimator.swift b/NearbyDemo/Managers/AnchorEstimator.swift new file mode 100644 index 0000000..751d2cd --- /dev/null +++ b/NearbyDemo/Managers/AnchorEstimator.swift @@ -0,0 +1,191 @@ +import Combine +import Foundation +import simd + +private struct Measurement { + let phonePosition: simd_float3 + let range: Float + let cameraForward: simd_float3 // unit vector: -Z column of camera transform at measurement time +} + +class AnchorEstimator: ObservableObject { + @Published var anchorPosition: simd_float3? = nil + @Published var measurementCount: Int = 0 + @Published var residualError: Float = 0 + @Published var offScreenAngle: Double? = nil + + private let windowSize = 50 + private let outlierThreshold: Float = 0.5 + private var measurements: [Measurement] = [] + private var currentEstimate: simd_float3? = nil + private var consecutiveOutliers: Int = 0 + // Skip outlier rejection for the first N measurements so the solver can converge + // before the gate starts locking it in place. + private var bootstrapCount: Int = 0 + private let bootstrapThreshold = 15 + + // MARK: - Public interface + + /// Feed a UWB range measurement paired with the camera pose at that moment. + /// cameraForward is the unit vector pointing in the camera's -Z direction (world frame). + func addMeasurement(phonePosition: simd_float3, range: Float, + cameraForward: simd_float3 = simd_float3(0, 0, -1)) { + bootstrapCount += 1 + let isBootstrapping = bootstrapCount <= bootstrapThreshold + + // Outlier rejection against current estimate (disabled during bootstrap so the solver + // can converge before the gate starts rejecting corrective measurements). + if !isBootstrapping, let est = currentEstimate { + let predicted = simd_length(phonePosition - est) + if abs(predicted - range) > outlierThreshold { + consecutiveOutliers += 1 + print("[Estimator] Rejected outlier (\(consecutiveOutliers)): predicted=\(predicted) measured=\(range)") + + if consecutiveOutliers >= 10 { + print("[Estimator] 10 consecutive outliers — estimate stuck. Resetting.") + reset() + } + return + } + } + + consecutiveOutliers = 0 + measurements.append(Measurement(phonePosition: phonePosition, range: range, cameraForward: cameraForward)) + if measurements.count > windowSize { + measurements.removeFirst() + } + + guard measurements.count >= 4 else { return } + + // Motion spread gate: require the phone to have moved meaningfully within the measurement + // window before running the solver. When all positions are nearly identical the system + // is underdetermined — the solver output is anywhere on a sphere and can jump arbitrarily. + let centroid = measurements.reduce(simd_float3.zero) { $0 + $1.phonePosition } / Float(measurements.count) + let rmsSpread = sqrt(measurements.reduce(Float(0)) { acc, m in + acc + simd_length_squared(m.phonePosition - centroid) + } / Float(measurements.count)) + + guard rmsSpread > 0.08 else { + // Phone hasn't moved enough — keep previous estimate without a new solve. + return + } + + currentEstimate = gaussNewton(measurements: measurements, initial: currentEstimate) + + let pos = currentEstimate! + let residuals = measurements.map { m -> Float in + let d = simd_length(m.phonePosition - pos) + return d - m.range + } + let rmse = sqrt(residuals.map { $0 * $0 }.reduce(0, +) / Float(residuals.count)) + + DispatchQueue.main.async { + self.anchorPosition = pos + self.measurementCount = self.measurements.count + self.residualError = rmse + } + } + + /// Directly set the anchor position from an external source (e.g. NI camera assistance). + func setKnownPosition(_ position: simd_float3) { + currentEstimate = position + DispatchQueue.main.async { + self.anchorPosition = position + } + } + + func reset() { + measurements.removeAll() + currentEstimate = nil + consecutiveOutliers = 0 + bootstrapCount = 0 + DispatchQueue.main.async { + self.anchorPosition = nil + self.measurementCount = 0 + self.residualError = 0 + } + } + + // MARK: - Gauss-Newton solver + + private func gaussNewton(measurements: [Measurement], initial: simd_float3?) -> simd_float3 { + var x: simd_float3 + if let prior = initial { + x = prior + } else { + // Initialize on the sphere centered at the most recent phone position, at the measured + // range, pointing in the camera-forward direction at the time of that measurement. + // This satisfies the most recent range constraint approximately and gives the solver + // a geometrically valid starting point rather than an arbitrary 1 m offset. + let lastM = measurements.last! + x = lastM.phonePosition + lastM.range * lastM.cameraForward + } + + let maxIterations = 10 + let convergenceThreshold: Float = 1e-4 + + for _ in 0.. 1e-6 else { continue } + + let residual = dist - m.range + if abs(residual) > outlierThreshold { continue } + + let J = -(diff / dist) // 1×3 Jacobian row + JtJ.columns.0 += simd_float3(J.x * J.x, J.y * J.x, J.z * J.x) + JtJ.columns.1 += simd_float3(J.x * J.y, J.y * J.y, J.z * J.y) + JtJ.columns.2 += simd_float3(J.x * J.z, J.y * J.z, J.z * J.z) + Jtf += J * residual + inlierCount += 1 + } + + guard inlierCount >= 3 else { break } + + // Tikhonov regularization (Levenberg-Marquardt style): keeps JᵀJ positive-definite + // when measurements are collinear, preventing numeric blow-up. + let lambda: Float = 1e-3 + JtJ.columns.0.x += lambda + JtJ.columns.1.y += lambda + JtJ.columns.2.z += lambda + + guard let JtJinv = inverse3x3(JtJ) else { break } + let delta = -(JtJinv * Jtf) + x += delta + + if simd_length(delta) < convergenceThreshold { break } + } + + return x + } + + /// Analytical 3×3 matrix inverse. Returns nil if the matrix is singular. + private func inverse3x3(_ m: simd_float3x3) -> simd_float3x3? { + let c0 = m.columns.0, c1 = m.columns.1, c2 = m.columns.2 + + let r0 = simd_float3( + c1.y * c2.z - c1.z * c2.y, + c0.z * c2.y - c0.y * c2.z, + c0.y * c1.z - c0.z * c1.y + ) + let r1 = simd_float3( + c1.z * c2.x - c1.x * c2.z, + c0.x * c2.z - c0.z * c2.x, + c0.z * c1.x - c0.x * c1.z + ) + let r2 = simd_float3( + c1.x * c2.y - c1.y * c2.x, + c0.y * c2.x - c0.x * c2.y, + c0.x * c1.y - c0.y * c1.x + ) + let det = c0.x * r0.x + c1.x * r0.y + c2.x * r0.z + guard abs(det) > 1e-10 else { return nil } + let invDet = 1.0 / det + return simd_float3x3(columns: (r0 * invDet, r1 * invDet, r2 * invDet)) + } +} diff --git a/NearbyDemo/Managers/BLEManager.swift b/NearbyDemo/Managers/BLEManager.swift new file mode 100644 index 0000000..8868747 --- /dev/null +++ b/NearbyDemo/Managers/BLEManager.swift @@ -0,0 +1,188 @@ +import CoreBluetooth +import Combine +import Foundation + +private extension Data { + var hexString: String { map { String(format: "%02x", $0) }.joined(separator: " ") } +} + +enum ConnectionState { + case disconnected, scanning, connecting, connected +} + +class BLEManager: NSObject, ObservableObject { + @Published var connectionState: ConnectionState = .disconnected + + /// Called with raw data when the TX characteristic notifies. + var onAccessoryData: ((Data) -> Void)? + /// Called when a peripheral successfully connects, passing its UUID. + var onConnected: ((UUID) -> Void)? + + /// The identifier of the currently connected peripheral, if any. + private(set) var connectedPeripheralIdentifier: UUID? + + private var centralManager: CBCentralManager! + private var peripheral: CBPeripheral? + private var rxCharacteristic: CBCharacteristic? + private var txCharacteristic: CBCharacteristic? + + // Whether to scan all peripherals (fallback) or filter by service UUID. + private var broadScan = false + + override init() { + super.init() + centralManager = CBCentralManager(delegate: self, queue: nil) + } + + func startScanning() { + guard centralManager.state == .poweredOn else { + // Will be triggered again in centralManagerDidUpdateState + return + } + connectionState = .scanning + // Try targeted scan first + broadScan = false + centralManager.scanForPeripherals(withServices: [QorvoBLEUUIDs.niService], options: nil) + print("[BLE] Scanning for NI service peripherals…") + + // Fall back to broad scan after 5 seconds if nothing found + DispatchQueue.main.asyncAfter(deadline: .now() + 5) { [weak self] in + guard let self, self.connectionState == .scanning else { return } + print("[BLE] No service-filtered result. Falling back to broad scan.") + self.broadScan = true + self.centralManager.stopScan() + self.centralManager.scanForPeripherals(withServices: nil, options: nil) + } + } + + func stopScanning() { + centralManager.stopScan() + } + + func sendToAccessory(_ data: Data) { + guard let peripheral, let rx = rxCharacteristic else { + print("[BLE] sendToAccessory: no peripheral/rx, dropping \(data.hexString)") + return + } + // Use withoutResponse for small messages only — the iOS BLE stack silently + // drops withoutResponse writes that exceed maximumWriteValueLength (often 20 + // bytes before MTU negotiation). For larger payloads (e.g. 0x0B + config) + // use withResponse, which the nRF52 SoftDevice will accept up to 247 bytes. + let maxNoRsp = peripheral.maximumWriteValueLength(for: .withoutResponse) + let canUseNoRsp = rx.properties.contains(.writeWithoutResponse) && data.count <= maxNoRsp + let writeType: CBCharacteristicWriteType = canUseNoRsp ? .withoutResponse : .withResponse + print("[BLE] → TX \(data.count) bytes (MTU max=\(maxNoRsp), type=\(writeType == .withResponse ? "rsp" : "no-rsp")): \(data.hexString)") + peripheral.writeValue(data, for: rx, type: writeType) + } + + func disconnect() { + guard let peripheral else { return } + centralManager.cancelPeripheralConnection(peripheral) + } +} + +// MARK: - CBCentralManagerDelegate +extension BLEManager: CBCentralManagerDelegate { + func centralManagerDidUpdateState(_ central: CBCentralManager) { + if central.state == .poweredOn { + startScanning() + } else { + connectionState = .disconnected + print("[BLE] Central state: \(central.state.rawValue)") + } + } + + func centralManager(_ central: CBCentralManager, + didDiscover peripheral: CBPeripheral, + advertisementData: [String: Any], + rssi RSSI: NSNumber) { + // When doing a broad scan, match by name prefix + if broadScan { + let name = peripheral.name ?? "" + guard name.hasPrefix("Qorvo") || name.hasPrefix("DWM") || name.contains("UWB") else { return } + } + + print("[BLE] Discovered: \(peripheral.name ?? "unknown") \(peripheral.identifier)") + centralManager.stopScan() + self.peripheral = peripheral + connectionState = .connecting + centralManager.connect(peripheral, options: nil) + } + + func centralManager(_ central: CBCentralManager, didConnect peripheral: CBPeripheral) { + print("[BLE] Connected to \(peripheral.name ?? peripheral.identifier.uuidString)") + connectionState = .connected + connectedPeripheralIdentifier = peripheral.identifier + peripheral.delegate = self + peripheral.discoverServices([QorvoBLEUUIDs.niService]) + // onConnected is deferred until TX notifications are confirmed enabled + } + + func centralManager(_ central: CBCentralManager, didFailToConnect peripheral: CBPeripheral, error: Error?) { + print("[BLE] Failed to connect: \(error?.localizedDescription ?? "unknown")") + connectionState = .disconnected + self.peripheral = nil + } + + func centralManager(_ central: CBCentralManager, didDisconnectPeripheral peripheral: CBPeripheral, error: Error?) { + print("[BLE] Disconnected: \(error?.localizedDescription ?? "clean")") + connectionState = .disconnected + connectedPeripheralIdentifier = nil + self.peripheral = nil + rxCharacteristic = nil + txCharacteristic = nil + } +} + +// MARK: - CBPeripheralDelegate +extension BLEManager: CBPeripheralDelegate { + func peripheral(_ peripheral: CBPeripheral, didDiscoverServices error: Error?) { + guard error == nil else { + print("[BLE] Service discovery error: \(error!)") + return + } + for service in peripheral.services ?? [] { + print("[BLE] Discovered service: \(service.uuid)") + peripheral.discoverCharacteristics([QorvoBLEUUIDs.rxCharacteristic, QorvoBLEUUIDs.txCharacteristic], for: service) + } + } + + func peripheral(_ peripheral: CBPeripheral, didDiscoverCharacteristicsFor service: CBService, error: Error?) { + guard error == nil else { + print("[BLE] Characteristic discovery error: \(error!)") + return + } + for characteristic in service.characteristics ?? [] { + print("[BLE] Discovered characteristic: \(characteristic.uuid) props: \(characteristic.properties.rawValue)") + if characteristic.uuid == QorvoBLEUUIDs.rxCharacteristic { + rxCharacteristic = characteristic + } else if characteristic.uuid == QorvoBLEUUIDs.txCharacteristic { + txCharacteristic = characteristic + peripheral.setNotifyValue(true, for: characteristic) + } + } + } + + func peripheral(_ peripheral: CBPeripheral, didUpdateNotificationStateFor characteristic: CBCharacteristic, error: Error?) { + if let error { + print("[BLE] Notify enable error on \(characteristic.uuid): \(error)") + return + } + guard characteristic.uuid == QorvoBLEUUIDs.txCharacteristic, characteristic.isNotifying else { return } + print("[BLE] TX notifications enabled — ready for NI handshake") + // Both characteristics discovered and notifications live: signal ready + onConnected?(peripheral.identifier) + } + + func peripheral(_ peripheral: CBPeripheral, didUpdateValueFor characteristic: CBCharacteristic, error: Error?) { + guard error == nil, let data = characteristic.value else { return } + print("[BLE] ← RX \(data.count) bytes: \(data.hexString)") + onAccessoryData?(data) + } + + func peripheral(_ peripheral: CBPeripheral, didWriteValueFor characteristic: CBCharacteristic, error: Error?) { + if let error { + print("[BLE] Write error on \(characteristic.uuid): \(error)") + } + } +} diff --git a/NearbyDemo/Managers/NIManager.swift b/NearbyDemo/Managers/NIManager.swift new file mode 100644 index 0000000..8feb630 --- /dev/null +++ b/NearbyDemo/Managers/NIManager.swift @@ -0,0 +1,248 @@ +import NearbyInteraction +import ARKit +import Combine +import Foundation +import CoreBluetooth +import QuartzCore +import simd + +enum SessionState: Equatable { + case idle + case waitingForAccessory // BLE ready, waiting for board to speak first + case configuring + case ranging + case error(String) +} + +// Apple NI Accessory Protocol — confirmed against DW3_QM33_SDK_1.1.1 firmware source +// (Src/Comm/Src/BLE/niq/ble_niq.c, Libs/niq/Inc/niq.h) +// +// Board → iOS (GATT notify on TX characteristic): +// 0x01 Accessory Configuration Data — board's UWB config, sent in reply to 0x0A +// 0x02 UWB Did Start +// 0x03 UWB Did Stop +// +// iOS → Board (GATT write to RX characteristic): +// 0x0A Init — iOS sends this first to request the board's UWB config +// 0x0B Configure and Start — iOS sends NI-generated shareable config +// 0x0C Stop +private enum MsgAccessoryToApp: UInt8 { + case accessoryConfigData = 0x01 + case uwbDidStart = 0x02 + case uwbDidStop = 0x03 +} + +private enum MsgAppToAccessory: UInt8 { + case init_ = 0x0A // iOS → Board: triggers board to send its UWB config + case configureAndStart = 0x0B // iOS → Board: send NI-generated shareable config + case stop = 0x0C // iOS → Board: stop ranging +} + +class NIManager: NSObject, ObservableObject { + @Published var sessionState: SessionState = .idle + @Published var lastRange: Float? = nil + + /// Called with (distance in meters, timestamp) on each range update (fallback when no camera assistance). + var onRangeUpdate: ((Float, TimeInterval) -> Void)? + /// Called with the accessory's world-space position when camera assistance resolves it. + var onWorldPositionUpdate: ((simd_float3) -> Void)? + /// Called with outbound BLE data to write to the accessory. + var sendToAccessory: ((Data) -> Void)? + + // Peripheral identifier set externally before start() is called. + var peripheralIdentifier: UUID? + // Set this to the app's ARSession before start() so NI can share it for camera assistance. + weak var arSession: ARSession? + + private var niSession: NISession? + private var restartWorkItem: DispatchWorkItem? + + /// Called by the app when BLE is connected and TX notifications are live. + /// Sends the MessageId_init (0x0A) command to trigger the board to reply + /// with its Accessory Configuration Data (0x01 + UWB config payload). + func start() { + guard sessionState == .idle else { return } + sessionState = .waitingForAccessory + let initMsg = Data([MsgAppToAccessory.init_.rawValue]) + sendToAccessory?(initMsg) + print("[NI] → Sent Init (0x0A) — waiting for board Accessory Config (0x01)") + } + + func handleAccessoryData(_ data: Data) { + guard !data.isEmpty else { return } + let tag = data[0] + let hex = data.map { String(format: "%02x", $0) }.joined(separator: " ") + print("[NI] ← rx tag=0x\(String(tag, radix: 16, uppercase: false)) len=\(data.count): \(hex)") + + switch tag { + case MsgAccessoryToApp.accessoryConfigData.rawValue: + // Board replied to our 0x0A with 0x01 + AccessoryConfigurationData payload + let payload = Data(data.dropFirst()) + guard !payload.isEmpty else { + print("[NI] 0x01 received but payload is empty — ignoring") + return + } + print("[NI] Board sent Accessory Config (0x01) — \(payload.count) bytes, starting NI session") + startNISession(with: payload) + + case MsgAccessoryToApp.uwbDidStart.rawValue: + print("[NI] Board confirmed UWB Did Start (0x02)") + DispatchQueue.main.async { self.sessionState = .ranging } + + case MsgAccessoryToApp.uwbDidStop.rawValue: + print("[NI] Board confirmed UWB Did Stop (0x03)") + DispatchQueue.main.async { + self.sessionState = .idle + self.lastRange = nil + } + + default: + print("[NI] Unexpected tag 0x\(String(tag, radix: 16, uppercase: false)) (\(data.count) bytes) — ignoring") + } + } + + func stop() { + restartWorkItem?.cancel() + sendToAccessory?(Data([MsgAppToAccessory.stop.rawValue])) + niSession?.invalidate() + niSession = nil + DispatchQueue.main.async { + self.sessionState = .idle + self.lastRange = nil + } + } + + // MARK: - Private + + private func startNISession(with accessoryData: Data) { + guard sessionState == .waitingForAccessory || sessionState == .idle else { + print("[NI] startNISession skipped — already in state \(sessionState)") + return + } + sessionState = .configuring + print("[NI] Creating NINearbyAccessoryConfiguration from \(accessoryData.count) bytes") + + // Use initWithData: (not initWithAccessoryData:bluetoothPeerIdentifier:). + // The peer-identifier variant requires a bonded/paired Bluetooth device; + // the DWM3001CDK is connected but not bonded, so that path silently prevents + // didGenerateShareableConfigurationData from ever firing. + // We handle all BLE transport manually, so the simple init is correct. + let configuration: NINearbyAccessoryConfiguration + do { + configuration = try NINearbyAccessoryConfiguration(data: accessoryData) + } catch { + let hexDump = accessoryData.map { String(format: "%02x", $0) }.joined(separator: " ") + print("[NI] Configuration parse error: \(error)\n data: \(hexDump)") + DispatchQueue.main.async { self.sessionState = .waitingForAccessory } + return + } + + // Enable camera assistance when the device supports it (iOS 16+, U1/U2 chip). + // This fuses ARKit visual-inertial odometry with UWB ranging inside Apple's framework, + // giving us a world-space position via worldTransform(for:) without running our own solver. + if #available(iOS 16.0, *), NISession.deviceCapabilities.supportsCameraAssistance { + configuration.isCameraAssistanceEnabled = true + print("[NI] Camera assistance enabled") + } + + let session = NISession() + session.delegate = self + niSession = session + + // Share our existing ARSession so NI doesn't spin up a second one. + // Must be called before run(_:). The session must already be running with a compatible config. + if #available(iOS 16.0, *), configuration.isCameraAssistanceEnabled, let arSession { + session.setARSession(arSession) + print("[NI] Shared ARSession with NISession") + } + + session.run(configuration) + print("[NI] NISession running") + } + + /// Send the Configure-and-Start message with both the canonical (0x0B) and + /// alternate (0x05) bytes so the board accepts it regardless of firmware version. + private func sendConfigureAndStart(_ shareableData: Data) { + // Try 0x0B first (Apple spec canonical byte) + var payload = Data([MsgAppToAccessory.configureAndStart.rawValue]) + payload.append(shareableData) + sendToAccessory?(payload) + print("[NI] → Sent Configure and Start (0x0B) \(payload.count) bytes") + } +} + +// MARK: - NISessionDelegate +extension NIManager: NISessionDelegate { + func session(_ session: NISession, didUpdate nearbyObjects: [NINearbyObject]) { + guard let object = nearbyObjects.first, let distance = object.distance else { return } + let timestamp = CACurrentMediaTime() + DispatchQueue.main.async { + self.lastRange = distance + self.sessionState = .ranging + } + + // Prefer camera-assisted world position: Apple's framework fuses UWB + ARKit VIO internally. + // worldTransform(for:) returns nil until camera assistance has converged. + if #available(iOS 16.0, *), + let worldTransform = session.worldTransform(for: object) { + let worldPos = simd_float3(worldTransform.columns.3.x, + worldTransform.columns.3.y, + worldTransform.columns.3.z) + onWorldPositionUpdate?(worldPos) + } else { + // Fall back to manual range+pose fusion via AnchorEstimator + onRangeUpdate?(distance, timestamp) + } + } + + func session(_ session: NISession, didRemove nearbyObjects: [NINearbyObject], reason: NINearbyObject.RemovalReason) { + print("[NI] Objects removed reason=\(reason.rawValue) — restarting NI handshake in 1 s") + DispatchQueue.main.async { + self.sessionState = .idle + self.lastRange = nil + } + // BLE is still connected — re-send 0x0A to restart the NI handshake + let work = DispatchWorkItem { [weak self] in + guard let self else { return } + DispatchQueue.main.async { self.sessionState = .idle } + self.start() + } + restartWorkItem = work + DispatchQueue.main.asyncAfter(deadline: .now() + 1, execute: work) + } + + func session(_ session: NISession, didUpdateAlgorithmConvergence convergence: NIAlgorithmConvergence, for object: NINearbyObject?) { + print("[NI] Algorithm convergence: \(convergence.status)") + } + + func session(_ session: NISession, didGenerateShareableConfigurationData shareableConfigurationData: Data, for object: NINearbyObject) { + print("[NI] didGenerateShareableConfigurationData — \(shareableConfigurationData.count) bytes") + sendConfigureAndStart(shareableConfigurationData) + } + + func sessionWasSuspended(_ session: NISession) { + print("[NI] Session suspended") + DispatchQueue.main.async { self.sessionState = .idle } + } + + func sessionSuspensionEnded(_ session: NISession) { + print("[NI] Suspension ended — rerunning configuration") + guard let config = session.configuration else { return } + session.run(config) + } + + func session(_ session: NISession, didInvalidateWith error: Error) { + print("[NI] Session invalidated: \(error)") + DispatchQueue.main.async { + self.sessionState = .error(error.localizedDescription) + self.niSession = nil + } + let work = DispatchWorkItem { [weak self] in + guard let self else { return } + DispatchQueue.main.async { self.sessionState = .idle } + self.start() + } + restartWorkItem = work + DispatchQueue.main.asyncAfter(deadline: .now() + 1, execute: work) + } +} diff --git a/NearbyDemo/Models/QorvoBLEUUIDs.swift b/NearbyDemo/Models/QorvoBLEUUIDs.swift new file mode 100644 index 0000000..379ad38 --- /dev/null +++ b/NearbyDemo/Models/QorvoBLEUUIDs.swift @@ -0,0 +1,9 @@ +import CoreBluetooth + +enum QorvoBLEUUIDs { + static let niService = CBUUID(string: "2E938FD0-6A61-11ED-A1EB-0242AC120002") + // App writes accessory configuration / commands to this characteristic + static let rxCharacteristic = CBUUID(string: "2E93998A-6A61-11ED-A1EB-0242AC120002") + // Accessory notifies the app with ranging data / responses on this characteristic + static let txCharacteristic = CBUUID(string: "2E939AF2-6A61-11ED-A1EB-0242AC120002") +} diff --git a/NearbyDemo/Models/TimestampedPose.swift b/NearbyDemo/Models/TimestampedPose.swift new file mode 100644 index 0000000..aa3d132 --- /dev/null +++ b/NearbyDemo/Models/TimestampedPose.swift @@ -0,0 +1,7 @@ +import simd +import Foundation + +struct TimestampedPose { + let transform: simd_float4x4 + let timestamp: TimeInterval +} diff --git a/NearbyDemo/Models/TimestampedRange.swift b/NearbyDemo/Models/TimestampedRange.swift new file mode 100644 index 0000000..f12457b --- /dev/null +++ b/NearbyDemo/Models/TimestampedRange.swift @@ -0,0 +1,6 @@ +import Foundation + +struct TimestampedRange { + let range: Float + let timestamp: TimeInterval +} diff --git a/NearbyDemo/NearbyDemoApp.swift b/NearbyDemo/NearbyDemoApp.swift new file mode 100644 index 0000000..f6858af --- /dev/null +++ b/NearbyDemo/NearbyDemoApp.swift @@ -0,0 +1,67 @@ +import SwiftUI +import ARKit +import simd + +@main +struct NearbyDemoApp: App { + @StateObject private var ble = BLEManager() + @StateObject private var ni = NIManager() + @StateObject private var ar = ARManager() + @StateObject private var estimator = AnchorEstimator() + + var body: some Scene { + WindowGroup { + ContentView() + .environmentObject(ble) + .environmentObject(ni) + .environmentObject(ar) + .environmentObject(estimator) + .onAppear { + wireManagers() + ar.start() + ble.startScanning() + } + } + } + + private func wireManagers() { + // BLE → NI: forward raw accessory bytes into the NI state machine + ble.onAccessoryData = { [weak ni] data in + ni?.handleAccessoryData(data) + } + + // NI → BLE: send outbound messages to the accessory + ni.sendToAccessory = { [weak ble] data in + ble?.sendToAccessory(data) + } + + // Share the ARSession with NI for camera assistance. Must be set before BLE connects + // and triggers startNISession, which calls setARSession(_:) before session.run(_:). + ni.arSession = ar.session + + // BLE connected → start NI session + ble.onConnected = { [weak ni] peripheralID in + ni?.peripheralIdentifier = peripheralID + ni?.start() + } + + // NI camera-assisted world position → set directly on estimator, bypassing Gauss-Newton. + // Apple's framework fuses UWB + ARKit VIO internally; this is more accurate than our solver. + ni.onWorldPositionUpdate = { [weak estimator] position in + estimator?.setKnownPosition(position) + } + + // NI range-only updates → fuse with AR pose → feed Gauss-Newton estimator. + // This runs when camera assistance hasn't converged yet or isn't supported. + ni.onRangeUpdate = { [weak ar, weak estimator] range, timestamp in + guard let ar, let estimator else { return } + guard let pose = ar.poseAt(timestamp: timestamp) else { return } + let position = simd_float3(pose.columns.3.x, pose.columns.3.y, pose.columns.3.z) + // Camera forward in world space: -Z column of the camera transform + let cameraForward = simd_normalize(simd_float3(-pose.columns.2.x, + -pose.columns.2.y, + -pose.columns.2.z)) + estimator.addMeasurement(phonePosition: position, range: range, cameraForward: cameraForward) + } + } +} diff --git a/NearbyDemo/Utilities/PoseInterpolator.swift b/NearbyDemo/Utilities/PoseInterpolator.swift new file mode 100644 index 0000000..9ef2d71 --- /dev/null +++ b/NearbyDemo/Utilities/PoseInterpolator.swift @@ -0,0 +1,27 @@ +import simd +import Foundation + +struct PoseInterpolator { + /// Interpolates between two timestamped poses at the given timestamp. + /// Uses linear interpolation for translation and spherical linear interpolation for rotation. + static func interpolate(from a: TimestampedPose, to b: TimestampedPose, at timestamp: TimeInterval) -> simd_float4x4 { + let t = Float((timestamp - a.timestamp) / (b.timestamp - a.timestamp)) + let clamped = max(0, min(1, t)) + return lerp(from: a.transform, to: b.transform, t: clamped) + } + + /// Interpolates between two simd_float4x4 transforms with a given t in [0, 1]. + static func lerp(from a: simd_float4x4, to b: simd_float4x4, t: Float) -> simd_float4x4 { + let tA = simd_float3(a.columns.3.x, a.columns.3.y, a.columns.3.z) + let tB = simd_float3(b.columns.3.x, b.columns.3.y, b.columns.3.z) + let qA = simd_quatf(a) + let qB = simd_quatf(b) + + let tInterp = tA + (tB - tA) * t + let qInterp = simd_slerp(qA, qB, t) + + var result = simd_float4x4(qInterp) + result.columns.3 = simd_float4(tInterp.x, tInterp.y, tInterp.z, 1.0) + return result + } +} diff --git a/NearbyDemoTests/NearbyDemoTests.swift b/NearbyDemoTests/NearbyDemoTests.swift new file mode 100644 index 0000000..27ebaa7 --- /dev/null +++ b/NearbyDemoTests/NearbyDemoTests.swift @@ -0,0 +1,19 @@ +// +// NearbyDemoTests.swift +// NearbyDemoTests +// +// Created by Aditya Pulipaka on 4/16/26. +// + +import Testing +@testable import NearbyDemo + +struct NearbyDemoTests { + + @Test func example() async throws { + // Write your test here and use APIs like `#expect(...)` to check expected conditions. + // Swift Testing Documentation + // https://developer.apple.com/documentation/testing + } + +} diff --git a/NearbyDemoUITests/NearbyDemoUITests.swift b/NearbyDemoUITests/NearbyDemoUITests.swift new file mode 100644 index 0000000..8ea7cd7 --- /dev/null +++ b/NearbyDemoUITests/NearbyDemoUITests.swift @@ -0,0 +1,43 @@ +// +// NearbyDemoUITests.swift +// NearbyDemoUITests +// +// Created by Aditya Pulipaka on 4/16/26. +// + +import XCTest + +final class NearbyDemoUITests: XCTestCase { + + override func setUpWithError() throws { + // Put setup code here. This method is called before the invocation of each test method in the class. + + // In UI tests it is usually best to stop immediately when a failure occurs. + continueAfterFailure = false + + // In UI tests it’s important to set the initial state - such as interface orientation - required for your tests before they run. The setUp method is a good place to do this. + } + + override func tearDownWithError() throws { + // Put teardown code here. This method is called after the invocation of each test method in the class. + } + + @MainActor + func testExample() throws { + // UI tests must launch the application that they test. + let app = XCUIApplication() + app.launch() + + // Use XCTAssert and related functions to verify your tests produce the correct results. + // XCUIAutomation Documentation + // https://developer.apple.com/documentation/xcuiautomation + } + + @MainActor + func testLaunchPerformance() throws { + // This measures how long it takes to launch your application. + measure(metrics: [XCTApplicationLaunchMetric()]) { + XCUIApplication().launch() + } + } +} diff --git a/NearbyDemoUITests/NearbyDemoUITestsLaunchTests.swift b/NearbyDemoUITests/NearbyDemoUITestsLaunchTests.swift new file mode 100644 index 0000000..e4b2783 --- /dev/null +++ b/NearbyDemoUITests/NearbyDemoUITestsLaunchTests.swift @@ -0,0 +1,35 @@ +// +// NearbyDemoUITestsLaunchTests.swift +// NearbyDemoUITests +// +// Created by Aditya Pulipaka on 4/16/26. +// + +import XCTest + +final class NearbyDemoUITestsLaunchTests: XCTestCase { + + override class var runsForEachTargetApplicationUIConfiguration: Bool { + true + } + + override func setUpWithError() throws { + continueAfterFailure = false + } + + @MainActor + func testLaunch() throws { + let app = XCUIApplication() + app.launch() + + // Insert steps here to perform after app launch but before taking a screenshot, + // such as logging into a test account or navigating somewhere in the app + // XCUIAutomation Documentation + // https://developer.apple.com/documentation/xcuiautomation + + let attachment = XCTAttachment(screenshot: app.screenshot()) + attachment.name = "Launch Screen" + attachment.lifetime = .keepAlways + add(attachment) + } +}