Stable tracking of DWM module

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()
+    }
+}

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..<maxIterations {
+            var JtJ = simd_float3x3(0)
+            var Jtf = simd_float3.zero
+            var inlierCount = 0
+
+            for m in measurements {
+                let diff = m.phonePosition - x
+                let dist = simd_length(diff)
+                guard dist > 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))
+    }
+}

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)")
+        }
+    }
+}

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)
+    }
+}