From 4863e3d313a600b003fcd504eff0d7648f5fc88e Mon Sep 17 00:00:00 2001 From: Sapphire Date: Sat, 22 Aug 2026 20:39:57 -0500 Subject: [PATCH] SolarXR external trackers --- .../java/dev/slimevr/android/ipc/ws-server.kt | 2 + .../main/java/dev/slimevr/device/module.kt | 3 + .../main/java/dev/slimevr/routing/routing.kt | 5 +- .../main/java/dev/slimevr/solarxr/common.kt | 14 +++ .../main/java/dev/slimevr/solarxr/datafeed.kt | 10 -- .../dev/slimevr/solarxr/driver/handshake.kt | 67 +++++++--- .../dev/slimevr/solarxr/driver/incoming.kt | 70 +++++++---- .../dev/slimevr/solarxr/driver/outgoing.kt | 117 ++++++------------ .../main/java/dev/slimevr/solarxr/module.kt | 40 +++--- .../dev/slimevr/tracker/behaviours/basic.kt | 2 + .../main/java/dev/slimevr/tracker/module.kt | 5 + .../java/dev/slimevr/desktop/ipc/solarxr.kt | 2 + solarxr-protocol | 2 +- 13 files changed, 187 insertions(+), 152 deletions(-) create mode 100644 server/core/src/main/java/dev/slimevr/solarxr/common.kt diff --git a/server/android/src/main/java/dev/slimevr/android/ipc/ws-server.kt b/server/android/src/main/java/dev/slimevr/android/ipc/ws-server.kt index 11c5e8a70..acb55dc3f 100644 --- a/server/android/src/main/java/dev/slimevr/android/ipc/ws-server.kt +++ b/server/android/src/main/java/dev/slimevr/android/ipc/ws-server.kt @@ -47,6 +47,8 @@ suspend fun createAndroidSolarXRWebsocketServer(appContext: AppContextProvider) send(Frame.Binary(fin = true, data = fbb.dataBuffer().moveToByteArray())) }.launchIn(this) + bridge.startObserving() + try { flow { for (frame in incoming) { diff --git a/server/core/src/main/java/dev/slimevr/device/module.kt b/server/core/src/main/java/dev/slimevr/device/module.kt index 0b1127cb2..52755398b 100644 --- a/server/core/src/main/java/dev/slimevr/device/module.kt +++ b/server/core/src/main/java/dev/slimevr/device/module.kt @@ -29,6 +29,7 @@ data class DeviceState( val protocolVersion: Int, val status: TrackerStatus, val origin: DeviceOrigin, + val driverName: String? = null, val packetsReceived: Long, val packetsLost: Long, ) @@ -57,6 +58,7 @@ class Device( address: String, macAddress: String? = null, origin: DeviceOrigin, + driverName: String? = null, protocolVersion: Int, ): Device { val deviceState = DeviceState( @@ -67,6 +69,7 @@ class Device( batteryVoltage = null, batteryRemainingRuntime = null, origin = origin, + driverName = driverName, address = address, macAddress = macAddress, protocolVersion = protocolVersion, diff --git a/server/core/src/main/java/dev/slimevr/routing/routing.kt b/server/core/src/main/java/dev/slimevr/routing/routing.kt index 67164c6df..5442576b4 100644 --- a/server/core/src/main/java/dev/slimevr/routing/routing.kt +++ b/server/core/src/main/java/dev/slimevr/routing/routing.kt @@ -60,11 +60,12 @@ fun driverStateFlow(appContext: AppContextProvider): Flow = appContext.server.context.state.map { state -> state.drivers.values.any { it.source == DriverBridgeSource.DRIVER } }, -) { enabled, connected -> + appContext.server.context.state.map { state -> state.solarxr.values.any { it.context.state.value.driverName != null } }.distinctUntilChanged(), +) { enabled, driverConnected, solarxrDriverConnected -> when { !appContext.featureFlags.supportsDriver -> RoutingOutputState.UNSUPPORTED !enabled -> RoutingOutputState.INACTIVE - connected -> RoutingOutputState.ACTIVE + driverConnected || solarxrDriverConnected -> RoutingOutputState.ACTIVE else -> RoutingOutputState.ENABLED } }.distinctUntilChanged() diff --git a/server/core/src/main/java/dev/slimevr/solarxr/common.kt b/server/core/src/main/java/dev/slimevr/solarxr/common.kt new file mode 100644 index 000000000..a3e67cc94 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/solarxr/common.kt @@ -0,0 +1,14 @@ +package dev.slimevr.solarxr + +import dev.slimevr.skeleton.BoneState +import solarxr_protocol.datatypes.BoneMask +import solarxr_protocol.datatypes.math.Quat +import solarxr_protocol.datatypes.math.Vec3f + +fun createBone(bone: BoneState, mask: BoneMask): solarxr_protocol.datatypes.Bone = solarxr_protocol.datatypes.Bone( + bodyPart = bone.bodyPart.takeIf { mask.bodyPart == true }, + orientationG = bone.orientation.let { Quat(it.x, it.y, it.z, it.w) }.takeIf { mask.orientationG == true }, + rotationG = bone.rotation.let { Quat(it.x, it.y, it.z, it.w) }.takeIf { mask.rotationG == true }, + boneLength = bone.offset.len().takeIf { mask.boneLength == true }, + headPositionG = bone.headPosition.let { Vec3f(it.x, it.y, it.z) }.takeIf { mask.headPositionG == true }, +) diff --git a/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt b/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt index d908c9939..adb9a5d35 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt @@ -6,7 +6,6 @@ import dev.slimevr.heightcalibration.HeightCalibrationManager import dev.slimevr.hid.HIDReceiverState import dev.slimevr.logging.AppLogger import dev.slimevr.resets.ResetsManager -import dev.slimevr.skeleton.BoneState import dev.slimevr.skeleton.Skeleton import dev.slimevr.tracker.Motion import dev.slimevr.tracker.TrackerState @@ -14,7 +13,6 @@ import kotlinx.coroutines.cancelAndJoin import kotlinx.coroutines.delay import kotlinx.coroutines.isActive import kotlinx.coroutines.launch -import solarxr_protocol.data_feed.BoneMask import solarxr_protocol.data_feed.DataFeedConfig import solarxr_protocol.data_feed.DataFeedMessageHeader import solarxr_protocol.data_feed.DataFeedUpdate @@ -117,14 +115,6 @@ private fun createDevice( ) } -private fun createBone(bone: BoneState, mask: BoneMask): solarxr_protocol.data_feed.Bone = solarxr_protocol.data_feed.Bone( - bodyPart = bone.bodyPart.takeIf { mask.bodyPart == true }, - orientationG = bone.orientation.let { Quat(it.x, it.y, it.z, it.w) }.takeIf { mask.orientationG == true }, - rotationG = bone.rotation.let { Quat(it.x, it.y, it.z, it.w) }.takeIf { mask.rotationG == true }, - boneLength = bone.offset.len().takeIf { mask.boneLength == true }, - headPositionG = bone.headPosition.let { Vec3f(it.x, it.y, it.z) }.takeIf { mask.headPositionG == true }, -) - private fun createServerGuards(resetsManager: ResetsManager, heightCalibrationManager: HeightCalibrationManager): ServerGuards { val resetsState = resetsManager.context.state.value val heightCalibrationState = heightCalibrationManager.context.state.value diff --git a/server/core/src/main/java/dev/slimevr/solarxr/driver/handshake.kt b/server/core/src/main/java/dev/slimevr/solarxr/driver/handshake.kt index c889dd0bf..463a60fed 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/driver/handshake.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/driver/handshake.kt @@ -1,43 +1,72 @@ package dev.slimevr.solarxr.driver -import dev.slimevr.VRServer +import dev.slimevr.AppContextProvider +import dev.slimevr.logging.AppLogger import dev.slimevr.solarxr.SolarXRBridge import dev.slimevr.solarxr.SolarXRBridgeActions import dev.slimevr.solarxr.SolarXRBridgeBehaviour import dev.slimevr.solarxr.SolarXRBridgeState -import solarxr_protocol.driver_protocol.DriverHandshakeStatus -import solarxr_protocol.driver_protocol.InboundHandshakeRequest -import solarxr_protocol.driver_protocol.InboundHandshakeResponse +import kotlinx.coroutines.flow.distinctUntilChanged +import kotlinx.coroutines.flow.launchIn +import kotlinx.coroutines.flow.map +import kotlinx.coroutines.flow.onEach +import solarxr_protocol.driver_protocol.HandshakeAvailable +import solarxr_protocol.driver_protocol.HandshakeRequest +import solarxr_protocol.driver_protocol.HandshakeResponse +import solarxr_protocol.driver_protocol.HandshakeStatus class DriverHandshakeBehaviour( - private val server: VRServer, + private val appContext: AppContextProvider, ) : SolarXRBridgeBehaviour { override fun reduce(state: SolarXRBridgeState, action: SolarXRBridgeActions) = when (action) { - is SolarXRBridgeActions.SetDriverName -> state.copy(driverName = action.name) + is SolarXRBridgeActions.SetDriverInfo -> state.copy(driverName = action.name, boneMask = action.boneMask) else -> state } override fun observe(receiver: SolarXRBridge) { - receiver.onDriverMessage { req, replyTo -> - val name = req.driverName + val server = appContext.server - val duplicate = name != null && - server.context.state.value.solarxr.values.any { - it.id != receiver.id && it.context.state.value.driverName == name + appContext.config.settings.context.state + .map { it.data.driverConfig.enabled } + .distinctUntilChanged() + .onEach { enabled -> + if (enabled) { + receiver.sendDriverMessage(HandshakeAvailable()) + } else { + val driverName = receiver.context.state.value.driverName ?: return@onEach + + AppLogger.solarxr.info("Disconnecting driver \"$driverName\"") + receiver.disconnectDriverTrackers() + receiver.context.dispatch(SolarXRBridgeActions.SetDriverInfo(null, null)) + receiver.sendDriverMessage(HandshakeResponse(status = HandshakeStatus.REJECTED_DISABLED)) } + }.launchIn(receiver.context.scope) - if (duplicate) { - receiver.sendDriverMessage( - InboundHandshakeResponse(status = DriverHandshakeStatus.REJECTED_DUPLICATE), - replyTo = replyTo, - ) - receiver.disconnect() + receiver.onDriverMessage { req, replyTo -> + val name = req.driverName?.takeIf { it.isNotEmpty() } ?: run { + AppLogger.solarxr.info("Rejecting driver handshake because it's unnamed") + receiver.sendDriverMessage(HandshakeResponse(status = HandshakeStatus.REJECTED_UNNAMED), replyTo = replyTo) return@onDriverMessage } - if (name != null) receiver.context.dispatch(SolarXRBridgeActions.SetDriverName(name)) + val duplicate = server.context.state.value.solarxr.values.any { + it.id != receiver.id && it.context.state.value.driverName == name + } + + if (duplicate) { + AppLogger.solarxr.info("Rejecting handshake from \"$name\" because it's a duplicate") + receiver.sendDriverMessage( + HandshakeResponse(status = HandshakeStatus.REJECTED_DUPLICATE), + replyTo = replyTo, + ) + receiver.context.dispatch(SolarXRBridgeActions.SetDriverInfo(null, null)) + return@onDriverMessage + } + + AppLogger.solarxr.info("Shook hands with \"$name\"") + receiver.context.dispatch(SolarXRBridgeActions.SetDriverInfo(name, req.boneMask)) receiver.sendDriverMessage( - InboundHandshakeResponse(status = DriverHandshakeStatus.ACCEPTED), + HandshakeResponse(status = HandshakeStatus.ACCEPTED), replyTo = replyTo, ) }.launchIn(receiver.context.scope) diff --git a/server/core/src/main/java/dev/slimevr/solarxr/driver/incoming.kt b/server/core/src/main/java/dev/slimevr/solarxr/driver/incoming.kt index b74f93c22..bed688a80 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/driver/incoming.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/driver/incoming.kt @@ -1,6 +1,6 @@ package dev.slimevr.solarxr.driver -import dev.slimevr.VRServer +import dev.slimevr.AppContextProvider import dev.slimevr.VRServerActions import dev.slimevr.device.Device import dev.slimevr.device.DeviceActions @@ -12,32 +12,48 @@ import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 import solarxr_protocol.datatypes.DeviceOrigin import solarxr_protocol.datatypes.TrackerStatus +import solarxr_protocol.driver_protocol.AddTrackerRequest +import solarxr_protocol.driver_protocol.AddTrackerResponse import solarxr_protocol.driver_protocol.AddTrackerStatus -import solarxr_protocol.driver_protocol.InboundAddTrackerRequest -import solarxr_protocol.driver_protocol.InboundAddTrackerResponse -import solarxr_protocol.driver_protocol.InboundBatteryNotification -import solarxr_protocol.driver_protocol.InboundTrackerPositionNotification -import solarxr_protocol.driver_protocol.InboundTrackerStatusNotification +import solarxr_protocol.driver_protocol.UpdateTrackerBattery +import solarxr_protocol.driver_protocol.UpdateTrackerPosition +import solarxr_protocol.driver_protocol.UpdateTrackerStatus class DriverIncomingTrackersBehaviour( - private val server: VRServer, + private val appContext: AppContextProvider, ) : SolarXRBridgeBehaviour { override fun observe(receiver: SolarXRBridge) { - receiver.onDriverMessage { req, replyTo -> - val hardwareId = req.hardwareId + val server = appContext.server - if (hardwareId == null || receiver.context.state.value.driverName == null) { - receiver.sendDriverMessage(InboundAddTrackerResponse(status = AddTrackerStatus.ERROR), replyTo = replyTo) + receiver.onDriverMessage { req, replyTo -> + val driverName = receiver.context.state.value.driverName + val hardwareId = req.hardwareIdentifier + + if (hardwareId == null || driverName == null) { + receiver.sendDriverMessage(AddTrackerResponse(status = AddTrackerStatus.ERROR), replyTo = replyTo) return@onDriverMessage } val existing = server.context.state.value.trackers.values .find { it.context.state.value.hardwareId == hardwareId } if (existing != null) { + val trackerState = existing.context.state.value + // Tracker is in use by another driver right now + if (trackerState.driverName != null && trackerState.driverName != driverName) { + receiver.sendDriverMessage(AddTrackerResponse(status = AddTrackerStatus.ERROR), replyTo = replyTo) + return@onDriverMessage + } + + existing.context.dispatchAll( + listOf( + TrackerActions.SetDriverName(driverName), + TrackerActions.SetStatus(TrackerStatus.OK), + ), + ) receiver.sendDriverMessage( - InboundAddTrackerResponse( + AddTrackerResponse( status = AddTrackerStatus.ALREADY_EXISTS, - trackerId = existing.context.state.value.id.toUShort(), + trackerId = trackerState.id.toUShort(), ), replyTo = replyTo, ) @@ -48,13 +64,14 @@ class DriverIncomingTrackersBehaviour( val deviceId = server.nextHandle() val device = Device.create( scope = scope, - appContext = receiver.appContext, + appContext = appContext, id = deviceId, - name = req.displayName ?: "Tracker", - manufacturer = req.manufacturer ?: "SlimeVR", + name = req.displayName ?: "Device #$deviceId", + manufacturer = req.manufacturer ?: "External", address = hardwareId, macAddress = hardwareId, origin = DeviceOrigin.DRIVER, + driverName = driverName, protocolVersion = 0, ) server.context.dispatch(VRServerActions.NewDevice(deviceId, device)) @@ -68,46 +85,47 @@ class DriverIncomingTrackersBehaviour( deviceId = deviceId, hardwareId = hardwareId, origin = DeviceOrigin.DRIVER, - appContext = receiver.appContext, + driverName = driverName, + appContext = appContext, ) server.context.dispatch(VRServerActions.NewTracker(trackerId, tracker)) tracker.context.dispatch(TrackerActions.SetStatus(TrackerStatus.OK)) receiver.sendDriverMessage( - InboundAddTrackerResponse(status = AddTrackerStatus.CREATED, trackerId = trackerId.toUShort()), + AddTrackerResponse(status = AddTrackerStatus.CREATED, trackerId = trackerId.toUShort()), replyTo = replyTo, ) }.launchIn(receiver.context.scope) - receiver.driverDispatcher.on { event -> + receiver.driverDispatcher.on { event -> if (receiver.context.state.value.driverName == null) return@on val trackerId = event.trackerId ?: return@on val status = event.status ?: return@on server.getTracker(trackerId.toInt())?.context?.dispatch(TrackerActions.SetStatus(status)) }.launchIn(receiver.context.scope) - receiver.driverDispatcher.on { event -> + receiver.driverDispatcher.on { event -> if (receiver.context.state.value.driverName == null) return@on val trackerId = event.trackerId ?: return@on - val batteryLevel = event.batteryLevel ?: return@on - val charging = event.charging ?: false val tracker = server.getTracker(trackerId.toInt()) ?: return@on val device = server.getDevice(tracker.context.state.value.deviceId) ?: return@on + + val batteryLevel = event.batteryLevel ?: return@on + val charging = event.charging ?: false device.context.dispatch( DeviceActions.Update { - copy(batteryLevel = batteryLevel / 100f, batteryVoltage = if (charging) 4.3f else 3.7f) + copy(batteryLevel = batteryLevel.toFloat() / 100f, batteryVoltage = if (charging) 4.3f else 3.7f) }, ) }.launchIn(receiver.context.scope) - receiver.driverDispatcher.on { event -> + receiver.driverDispatcher.on { event -> if (receiver.context.state.value.driverName == null) return@on val trackerId = event.trackerId ?: return@on - val rotation = event.rotation ?: return@on server.getTracker(trackerId.toInt())?.context?.dispatch( TrackerActions.SetRotation( - rotation = Quaternion(rotation.w, rotation.x, rotation.y, rotation.z), + rotation = event.rotation?.let { Quaternion(it.w, it.x, it.y, it.z) }, position = event.position?.let { Vector3(it.x, it.y, it.z) }, // TODO: send velocity? ), diff --git a/server/core/src/main/java/dev/slimevr/solarxr/driver/outgoing.kt b/server/core/src/main/java/dev/slimevr/solarxr/driver/outgoing.kt index e41cecd6c..dbeb67d2c 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/driver/outgoing.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/driver/outgoing.kt @@ -6,26 +6,20 @@ import dev.slimevr.skeleton.BodyPartMap import dev.slimevr.skeleton.bodyPartMap import dev.slimevr.solarxr.SolarXRBridge import dev.slimevr.solarxr.SolarXRBridgeBehaviour +import dev.slimevr.solarxr.createBone import dev.slimevr.tracker.TrackerState -import kotlinx.coroutines.TimeoutCancellationException import kotlinx.coroutines.flow.combine import kotlinx.coroutines.flow.distinctUntilChanged +import kotlinx.coroutines.flow.emptyFlow +import kotlinx.coroutines.flow.flatMapLatest import kotlinx.coroutines.flow.launchIn import kotlinx.coroutines.flow.map import kotlinx.coroutines.flow.onEach -import kotlinx.coroutines.launch import solarxr_protocol.datatypes.BodyPart import solarxr_protocol.datatypes.DeviceOrigin import solarxr_protocol.datatypes.TrackerStatus -import solarxr_protocol.datatypes.math.Quat -import solarxr_protocol.datatypes.math.Vec3f -import solarxr_protocol.driver_protocol.AddTrackerStatus -import solarxr_protocol.driver_protocol.OutboundAddTrackerRequest -import solarxr_protocol.driver_protocol.OutboundAddTrackerResponse -import solarxr_protocol.driver_protocol.OutboundTrackerPositionNotification -import solarxr_protocol.driver_protocol.OutboundTrackerStatusNotification -import solarxr_protocol.rpc.RoutingOutput -import java.util.concurrent.ConcurrentHashMap +import solarxr_protocol.driver_protocol.BoneBatteryUpdate +import solarxr_protocol.driver_protocol.SkeletonUpdate class DriverOutgoingTrackersBehaviour( private val appContext: AppContextProvider, @@ -48,89 +42,52 @@ class DriverOutgoingTrackersBehaviour( ) override fun observe(receiver: SolarXRBridge) { - val requestedTrackers = ConcurrentHashMap.newKeySet() - val confirmedTrackers = ConcurrentHashMap.newKeySet() + val server = appContext.server + val settings = appContext.config.settings - // Status and battery are rebuilt every frame but only change every few seconds, so the driver - // was being sent the same values at skeleton rate. Keep the last one per body part and only - // send on a real change, the same way subscribedTrackers already gates TrackerAdded. - val lastStatus = mutableMapOf() + val boneBatteries = bodyPartMap() - combine( - appContext.skeleton.computed, - appContext.boneRouting.context.state - .map { state -> state.routes.filterValues { RoutingOutput.DRIVER in it }.keys } - .distinctUntilChanged(), - ::Pair, - ) + combine(settings.context.state.map { it.data.driverConfig.enabled }, receiver.context.state) { enabled, state -> + Triple( + enabled, + state.driverName, + state.boneMask, + ) + } .distinctUntilChanged() - .onEach { (computedSkeleton, enabledBodyParts) -> - if (receiver.context.state.value.driverName == null) return@onEach - - val serverState = appContext.server.context.state.value - + .flatMapLatest { (enabled, driverName, boneMask) -> + if (!enabled || driverName == null || boneMask == null) return@flatMapLatest emptyFlow() // Map the nearest trackers to their body parts val trackerStateByBodyPart = bodyPartMap() - for (tracker in serverState.trackers.values) { + for (tracker in server.context.state.value.trackers.values) { val trackerState = tracker.context.state.value if (trackerState.origin == DeviceOrigin.DRIVER) continue val bodyPart = trackerState.bodyPart ?: continue trackerStateByBodyPart.putIfAbsent(bodyPart, trackerState) } - computedSkeleton.forEach { (part, state) -> - if (enabledBodyParts.contains(part)) { - val closestTracker = bodyPartToNearest[part].orEmpty() - .firstNotNullOfOrNull { fallbackPart -> trackerStateByBodyPart[fallbackPart] } - val closestDevice = serverState.devices[closestTracker?.deviceId]?.context?.state?.value + appContext.skeleton.computed.onEach { computedSkeleton -> + val bones = computedSkeleton.values.map { createBone(it, boneMask) } - if (requestedTrackers.add(part.value)) { - receiver.context.scope.launch { - try { - val response = receiver.requestDriverMessage( - OutboundAddTrackerRequest(trackerId = part.value.toUShort(), bodyPart = part), - ) - if (response.status == AddTrackerStatus.ERROR) { - AppLogger.solarxr.warn("Driver rejected adding tracker for body part $part") - requestedTrackers.remove(part.value) // Should we retry on the next frame? - } else { - confirmedTrackers.add(part.value) - } - } catch (e: TimeoutCancellationException) { - AppLogger.solarxr.warn("Timeout waiting for driver to add tracker for body part $part") - requestedTrackers.remove(part.value) - } + receiver.sendDriverMessage(SkeletonUpdate(bones = bones)) + + computedSkeleton.keys.forEach { bodyPart -> + val closestTracker = bodyPartToNearest[bodyPart].orEmpty() + .firstNotNullOfOrNull { fallbackPart -> trackerStateByBodyPart[fallbackPart] } + val closestDevice = + server.context.state.value.devices[closestTracker?.deviceId]?.context?.state?.value + + if (closestDevice?.batteryLevel != null) { + val battery = BoneBatteryUpdate( + bone = bodyPart, + batteryLevel = (closestDevice.batteryLevel * 100).toUInt().toUByte(), + charging = closestDevice.batteryVoltage != null && closestDevice.batteryVoltage >= 4.3f, + ) + if (boneBatteries.put(bodyPart, battery) != battery) { + AppLogger.solarxr.debug("Sending BoneBatteryUpdate for $bodyPart") + receiver.sendDriverMessage(battery) } } - - if (part.value !in confirmedTrackers) return@forEach - - receiver.sendDriverMessage( - OutboundTrackerPositionNotification( - trackerId = part.value.toUShort(), - rotation = Quat(state.rotation.x, state.rotation.y, state.rotation.z, state.rotation.w), - position = Vec3f(state.tailPosition.x, state.tailPosition.y, state.tailPosition.z), - // TODO add veliocity data - ), - ) - - val status = OutboundTrackerStatusNotification( - trackerId = part.value.toUShort(), - status = closestTracker?.status ?: TrackerStatus.OK, - batteryLevel = closestDevice?.batteryLevel ?: 1f, - charging = closestDevice?.batteryVoltage != null && closestDevice.batteryVoltage >= 4.3f, - ) - if (lastStatus.put(part.value, status) != status) receiver.sendDriverMessage(status) - } else { - if (part.value !in confirmedTrackers) return@forEach - - val status = OutboundTrackerStatusNotification( - trackerId = part.value.toUShort(), - status = TrackerStatus.DISCONNECTED, - batteryLevel = null, - charging = false, - ) - if (lastStatus.put(part.value, status) != status) receiver.sendDriverMessage(status) } } }.launchIn(receiver.context.scope) diff --git a/server/core/src/main/java/dev/slimevr/solarxr/module.kt b/server/core/src/main/java/dev/slimevr/solarxr/module.kt index 1777385ac..060c4042e 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/module.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/module.kt @@ -17,6 +17,7 @@ import solarxr_protocol.MessageBundle import solarxr_protocol.data_feed.DataFeedConfig import solarxr_protocol.data_feed.DataFeedMessage import solarxr_protocol.data_feed.DataFeedMessageHeader +import solarxr_protocol.datatypes.BoneMask import solarxr_protocol.datatypes.DeviceOrigin import solarxr_protocol.datatypes.TrackerStatus import solarxr_protocol.driver_protocol.DriverMessage @@ -29,11 +30,12 @@ import kotlin.time.Duration.Companion.seconds data class SolarXRBridgeState( val dataFeedConfigs: List, val driverName: String? = null, + val boneMask: BoneMask? = null, ) sealed interface SolarXRBridgeActions { data class SetConfig(val configs: List) : SolarXRBridgeActions - data class SetDriverName(val name: String) : SolarXRBridgeActions + data class SetDriverInfo(val name: String?, val boneMask: BoneMask?) : SolarXRBridgeActions } typealias SolarXRBridgeContext = Context @@ -72,8 +74,8 @@ class SolarXRBridge( val id: Int, val context: SolarXRBridgeContext, val appContext: AppContextProvider, - val dataFeedDispatcher: EventDispatcher, - val rpcDispatcher: EventDispatcher, + val dataFeedDispatcher: EventDispatcher = EventDispatcher("SolarXR[$id].dataFeed", context.scope, capacity = 32), + val rpcDispatcher: EventDispatcher = EventDispatcher("SolarXR[$id].rpc", context.scope, capacity = 64), val driverDispatcher: EventDispatcher = EventDispatcher("SolarXR[$id].driver", context.scope, capacity = 64), val outbound: EventDispatcher = EventDispatcher("SolarXR[$id].outbound", context.scope, capacity = 64), private val managedContext: ManagedContext? = null, @@ -104,16 +106,29 @@ class SolarXRBridge( suspend fun sendDataFeed(frame: DataFeedMessageHeader) = outbound.emit(MessageBundle(dataFeedMsgs = listOf(frame))) + fun disconnectDriverTrackers() { + val driverName = context.state.value.driverName ?: return + // If the connection was a driver connection, we set the status of its trackers to disconnected + appContext.server.context.state.value.trackers.values + .filter { + val state = it.context.state.value + state.origin == DeviceOrigin.DRIVER && state.driverName == driverName + } + .forEach { + it.context.dispatchAll( + listOf( + TrackerActions.SetDriverName(null), + TrackerActions.SetStatus(TrackerStatus.DISCONNECTED), + ), + ) + } + } + fun disconnect() { dispose() appContext.server.context.dispatch(VRServerActions.SolarXRDisconnected(id)) - // if the connection was a driver connection. we set the trackers status to disconnected - if (context.state.value.driverName.isNullOrEmpty()) { - appContext.server.context.state.value.trackers.values.filter { it.context.state.value.origin == DeviceOrigin.DRIVER }.forEach { - it.context.dispatch(TrackerActions.SetStatus(status = TrackerStatus.DISCONNECTED)) - } - } + disconnectDriverTrackers() } fun startObserving() = context.observeAll(this) @@ -140,9 +155,9 @@ class SolarXRBridge( add(TrackingChecklistBehaviour(appContext.trackingChecklist, appContext.config.settings)) add(AssignTrackerBehaviour(appContext.server)) add(DongleSettingsBehaviour(appContext.server)) - add(DriverHandshakeBehaviour(appContext.server)) + add(DriverHandshakeBehaviour(appContext)) add(DriverOutgoingTrackersBehaviour(appContext)) - add(DriverIncomingTrackersBehaviour(appContext.server)) + add(DriverIncomingTrackersBehaviour(appContext)) add(MagBehaviour(appContext)) add(TimeoutSettingsBehaviour(appContext.config.settings)) add(KnownTrackersBehaviour(appContext.config.settings)) @@ -170,11 +185,8 @@ class SolarXRBridge( id = id, context = managedContext.context, appContext = appContext, - dataFeedDispatcher = EventDispatcher("SolarXR[$id].datafeed", managedContext.context.scope, capacity = 32), - rpcDispatcher = EventDispatcher("SolarXR[$id].rpc", managedContext.context.scope, capacity = 64), managedContext = managedContext, ) - bridge.startObserving() return bridge } } diff --git a/server/core/src/main/java/dev/slimevr/tracker/behaviours/basic.kt b/server/core/src/main/java/dev/slimevr/tracker/behaviours/basic.kt index 33ddbfa0a..47871f423 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/behaviours/basic.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/behaviours/basic.kt @@ -43,6 +43,8 @@ class TrackerBasicBehaviour(private val settings: Settings) : TrackerBehaviour { is TrackerActions.SetStatus -> state.copy(status = action.status) + is TrackerActions.SetDriverName -> state.copy(driverName = action.driverName) + is TrackerActions.SetRotation -> { // This action counts as a tick towards TPS if it has new rotation data. if (action.newData && action.rotation != null) tpsCount.incrementAndFetch() diff --git a/server/core/src/main/java/dev/slimevr/tracker/module.kt b/server/core/src/main/java/dev/slimevr/tracker/module.kt index 55188e782..213ad6248 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/module.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/module.kt @@ -51,6 +51,7 @@ data class TrackerState( val id: Int, val deviceId: Int, val origin: DeviceOrigin, + val driverName: String?, val hardwareId: String, val name: String, val imuType: ImuType?, @@ -97,6 +98,7 @@ sealed interface TrackerActions { data class Update(val transform: TrackerState.() -> TrackerState) : TrackerActions data class SetMagStatus(val status: MagnetometerStatus) : TrackerActions data class SetStatus(val status: TrackerStatus) : TrackerActions + data class SetDriverName(val driverName: String?) : TrackerActions data class SetRotation(val rotation: Quaternion? = null, val acceleration: Vector3? = null, val magnetometer: Vector3? = null, val position: Vector3? = null, val newData: Boolean = true) : TrackerActions data class SetMountingOrientation(val mountingOrientation: HeadingAlignment) : TrackerActions data class SetRestOrientation(val restOrientation: Quaternion) : TrackerActions @@ -129,6 +131,7 @@ class Tracker( imuType: ImuType? = null, hardwareId: String, origin: DeviceOrigin, + driverName: String? = null, appContext: AppContextProvider, ): Tracker { val settings = appContext.config.settings @@ -138,6 +141,7 @@ class Tracker( id = id, deviceId = deviceId, origin = origin, + driverName = driverName, hardwareId = hardwareId, name = name, imuType = imuType, @@ -178,6 +182,7 @@ class Tracker( id = 0, deviceId = 0, origin = DeviceOrigin.UDP, + driverName = null, hardwareId = "defaultHardwareId", name = "defaultTracker", imuType = ImuType.BNO085, diff --git a/server/desktop/src/main/java/dev/slimevr/desktop/ipc/solarxr.kt b/server/desktop/src/main/java/dev/slimevr/desktop/ipc/solarxr.kt index ed90ecf0f..776eeacf9 100644 --- a/server/desktop/src/main/java/dev/slimevr/desktop/ipc/solarxr.kt +++ b/server/desktop/src/main/java/dev/slimevr/desktop/ipc/solarxr.kt @@ -87,6 +87,8 @@ suspend fun handleSolarXRBridge( send(sendBuffer) }.launchIn(this) + bridge.startObserving() + val receiveArray = ByteArray(MAX_FRAME_SIZE) try { diff --git a/solarxr-protocol b/solarxr-protocol index b87124477..4ce533cb7 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit b87124477f92d1668ec17c00489263e6ac25272e +Subproject commit 4ce533cb7bfafc534f2422ad5d453995c9a9ad71