From aa505d041b2aee13ee4467e65401a313541fd382 Mon Sep 17 00:00:00 2001 From: loucass003 Date: Tue, 21 Apr 2026 01:31:24 +0200 Subject: [PATCH] Working mag. That was awfull --- .../src/main/java/dev/slimevr/solarxr/mag.kt | 81 ++++------ .../java/dev/slimevr/tracker/behaviours.kt | 10 +- .../main/java/dev/slimevr/tracker/config.kt | 9 +- .../slimevr/trackingchecklist/behaviours.kt | 2 +- .../main/java/dev/slimevr/udp/behaviours.kt | 153 +++++++++--------- .../main/java/dev/slimevr/udp/connection.kt | 3 +- 6 files changed, 120 insertions(+), 138 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/solarxr/mag.kt b/server/core/src/main/java/dev/slimevr/solarxr/mag.kt index 704972175..40648e143 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/mag.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/mag.kt @@ -5,81 +5,62 @@ import dev.slimevr.config.SettingsActions import dev.slimevr.device.DeviceOrigin import dev.slimevr.udp.SensorConfigFlags import dev.slimevr.udp.UDPConnectionActions -import kotlinx.coroutines.CoroutineScope -import kotlinx.coroutines.Job import kotlinx.coroutines.flow.distinctUntilChangedBy -import kotlinx.coroutines.flow.filter import kotlinx.coroutines.flow.first -import kotlinx.coroutines.flow.launchIn -import kotlinx.coroutines.flow.onEach import kotlinx.coroutines.launch import kotlinx.coroutines.withTimeout -import solarxr_protocol.datatypes.DeviceId import solarxr_protocol.datatypes.MagnetometerStatus -import solarxr_protocol.datatypes.TrackerId import solarxr_protocol.rpc.ChangeMagToggleRequest -import solarxr_protocol.rpc.ChangeSettingsRequest import solarxr_protocol.rpc.MagToggleRequest import solarxr_protocol.rpc.MagToggleResponse class MagBehaviour( private val appContext: AppContextProvider, ) : SolarXRBridgeBehaviour { - override fun observe(receiver: SolarXRBridge) { + private fun setUDPTrackerMag(trackerId: Int, deviceId: Int, enable: Boolean): Boolean { + val connection = appContext.udpServer.findConnectionForDevice(deviceId) ?: return false + val sensorId = connection.context.state.value.trackerIds.find { it.id == trackerId }?.trackerNum ?: return false + connection.context.dispatch( + UDPConnectionActions.SetSensorConfig( + sensorId = sensorId, + flags = SensorConfigFlags(magStatus = if (enable) MagnetometerStatus.ENABLED else MagnetometerStatus.DISABLED), + ) + ) + return true + } + override fun observe(receiver: SolarXRBridge) { receiver.rpcDispatcher.on { req -> val trackerId = req.trackerId if (trackerId == null) { - receiver.appContext.config.settings.context.dispatch( - SettingsActions.Update { - copy(globalMagEnabled = req.enable) + receiver.appContext.config.settings.context.dispatch(SettingsActions.Update { copy(globalMagEnabled = req.enable) }) + appContext.server.context.state.value.trackers.values.forEach { tracker -> + val state = tracker.context.state.value + if (state.magStatus == MagnetometerStatus.NOT_SUPPORTED) return@forEach + when (state.origin) { + DeviceOrigin.UDP -> setUDPTrackerMag(state.id, state.deviceId, req.enable) + DeviceOrigin.HID -> { /* TODO: implement HID mag toggle */ } + else -> Unit } - ) - receiver.sendRpc(MagToggleResponse( - trackerId = null, - enable = req.enable - )) + } + receiver.sendRpc(MagToggleResponse(trackerId = null, enable = req.enable)) return@on } val tracker = appContext.server.getTracker(trackerId.trackerNum.toInt()) ?: return@on val trackerState = tracker.context.state.value - if (trackerState.magStatus == MagnetometerStatus.NOT_SUPPORTED) return@on when (trackerState.origin) { DeviceOrigin.UDP -> { - val connection = appContext.udpServer.findConnectionForDevice(trackerState.deviceId) ?: return@on - val sensorId = connection.context.state.value.trackerIds - .find { it.id == trackerState.id } - ?.trackerNum - ?: return@on - connection.context.dispatch( - UDPConnectionActions.SetSensorConfig( - sensorId = sensorId, - flags = SensorConfigFlags(magEnabled = req.enable) - ), - ) - + if (!setUDPTrackerMag(trackerState.id, trackerState.deviceId, req.enable)) return@on tracker.context.scope.launch { withTimeout(10_000) { tracker.context.state .distinctUntilChangedBy { it.magStatus } - .first { trackerState -> - val status = when (trackerState.magStatus) { - MagnetometerStatus.ENABLED -> true - MagnetometerStatus.DISABLED -> false - MagnetometerStatus.NOT_SUPPORTED -> false - } - return@first status == req.enable - } - receiver.sendRpc( - MagToggleResponse( - trackerId = trackerId, - enable = req.enable - ) - ) + .first { it.magStatus == if (req.enable) MagnetometerStatus.ENABLED else MagnetometerStatus.DISABLED } + receiver.sendRpc(MagToggleResponse(trackerId = trackerId, enable = req.enable)) } } } @@ -94,23 +75,17 @@ class MagBehaviour( val trackerId = req.trackerId if (trackerId == null) { - val globalMagEnabled = receiver.appContext.config.settings.context.state.value.data.globalMagEnabled receiver.sendRpc(MagToggleResponse( trackerId = null, - enable = globalMagEnabled + enable = receiver.appContext.config.settings.context.state.value.data.globalMagEnabled, )) return@on } - val tracker = appContext.server.getTracker(trackerId.trackerNum.toInt()) ?: return@on - val trackerState = tracker.context.state.value + val trackerState = appContext.server.getTracker(trackerId.trackerNum.toInt())?.context?.state?.value ?: return@on receiver.sendRpc(MagToggleResponse( trackerId = trackerId, - enable = when (trackerState.magStatus) { - MagnetometerStatus.ENABLED -> true - MagnetometerStatus.DISABLED -> false - MagnetometerStatus.NOT_SUPPORTED -> false - } + enable = trackerState.magStatus == MagnetometerStatus.ENABLED, )) } } diff --git a/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt b/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt index 5f05f45b1..dca2247b1 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/behaviours.kt @@ -81,7 +81,15 @@ object TrackerTapDetectionBehaviour : TrackerBehaviour { } object TrackerBasicBehaviour : TrackerBehaviour { - override fun reduce(state: TrackerState, action: TrackerActions) = if (action is TrackerActions.Update) action.transform(state) else state + override fun reduce(state: TrackerState, action: TrackerActions) : TrackerState { + val newState = if (action is TrackerActions.Update) + action.transform(state) + else + state + if (action is TrackerActions.Update) + println(newState) + return newState + } } object TrackerTPSBehaviour : TrackerBehaviour { diff --git a/server/core/src/main/java/dev/slimevr/tracker/config.kt b/server/core/src/main/java/dev/slimevr/tracker/config.kt index 45b681764..a51a4a184 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/config.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/config.kt @@ -25,6 +25,11 @@ private fun applyStateToConfig(config: TrackerConfig, state: TrackerState) = con bodyPart = state.bodyPart, customName = state.customName, mountingOrientation = state.mountingOrientation, + magEnabled = when (state.magStatus) { + MagnetometerStatus.ENABLED -> true + MagnetometerStatus.DISABLED -> false + MagnetometerStatus.NOT_SUPPORTED -> config.magEnabled + }, ) class TrackerConfigBehaviour( @@ -33,11 +38,11 @@ class TrackerConfigBehaviour( ) : TrackerBehaviour { override fun observe(receiver: Tracker) { receiver.context.state - .distinctUntilChangedBy { it.bodyPart to it.customName to it.mountingOrientation } + .distinctUntilChangedBy { it.bodyPart to it.customName to it.mountingOrientation to it.magStatus } .drop(1) .onEach { state -> settings.context.dispatch(SettingsActions.UpdateTracker(hardwareId) { applyStateToConfig(this, state) }) } .launchIn(receiver.context.scope) } -} \ No newline at end of file +} diff --git a/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt b/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt index 15840665d..50ba339f7 100644 --- a/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt @@ -101,7 +101,7 @@ class TrackerRestCheckBehaviour(private val server: VRServer) : TrackingChecklis private fun computeStep(trackers: List): TrackingChecklistStep { val uncalibratedTrackers = trackers.filter { tracker -> (tracker.origin == DeviceOrigin.UDP || tracker.origin == DeviceOrigin.HID) - && tracker.status == TrackerStatus.OK && !tracker.completedRestCalibration + && tracker.status == TrackerStatus.OK && (tracker.completedRestCalibration != null && !tracker.completedRestCalibration) } return TrackingChecklistStep( valid = uncalibratedTrackers.isEmpty(), diff --git a/server/core/src/main/java/dev/slimevr/udp/behaviours.kt b/server/core/src/main/java/dev/slimevr/udp/behaviours.kt index 4328cb1ed..2a059fe86 100644 --- a/server/core/src/main/java/dev/slimevr/udp/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/udp/behaviours.kt @@ -229,6 +229,44 @@ object DeviceStatsBehaviour : UDPConnectionBehaviour { } object SensorInfoBehaviour : UDPConnectionBehaviour { + private suspend fun assignTracker(receiver: UDPConnection, device: Device, event: PacketEvent): Pair { + val deviceState = device.context.state.value + val mac = deviceState.macAddress ?: run { + AppLogger.udp.warn("[${deviceState.address}] No MAC address available, falling back to IP for hardware ID") + deviceState.address + } + val hardwareId = "$mac:${event.data.sensorId}" + + val existingTracker = receiver.appContext.server.context.state.value.trackers.values + .find { t -> t.context.state.value.deviceId == deviceState.id && t.context.state.value.hardwareId == hardwareId } + + if (existingTracker != null) { + receiver.context.dispatch( + UDPConnectionActions.AssignTracker( + trackerId = TrackerIdNum(id = existingTracker.context.state.value.id, trackerNum = event.data.sensorId), + ), + ) + return existingTracker to false + } + + val trackerId = receiver.appContext.server.nextHandle() + val newTracker = Tracker.create( + id = trackerId, + hardwareId = hardwareId, + sensorType = event.data.imuType, + deviceId = deviceState.id, + origin = DeviceOrigin.UDP, + scope = receiver.appContext.server.context.scope, + server = receiver.appContext.server, + settings = receiver.appContext.config.settings, + ) + receiver.appContext.server.context.dispatch(VRServerActions.NewTracker(trackerId = trackerId, context = newTracker)) + receiver.context.dispatch( + UDPConnectionActions.AssignTracker(trackerId = TrackerIdNum(id = trackerId, trackerNum = event.data.sensorId)), + ) + return newTracker to true + } + override fun reduce(state: UDPConnectionState, action: UDPConnectionActions) = when (action) { is UDPConnectionActions.AssignTracker -> state.copy(trackerIds = state.trackerIds + action.trackerId) else -> state @@ -236,93 +274,45 @@ object SensorInfoBehaviour : UDPConnectionBehaviour { override fun observe(receiver: UDPConnection) { receiver.packetEvents.onPacket { event -> - val device = receiver.getDevice() - ?: error("invalid state - a device should exist at this point") + val device = receiver.getDevice() ?: error("invalid state - a device should exist at this point") - val tracker = receiver.getTracker(event.data.sensorId) - val trackerState = tracker?.context?.state?.value + val existingTracker = receiver.getTracker(event.data.sensorId) + if (existingTracker != null) { + existingTracker.context.dispatch(TrackerActions.Update { + copy(sensorType = event.data.imuType, status = event.data.status, completedRestCalibration = event.data.hasCompletedRestCalibration) + }) + return@onPacket + } - val completedRestCalibration = event.data.hasCompletedRestCalibration + val (tracker, isNew) = assignTracker(receiver, device, event) + tracker.context.dispatch(TrackerActions.Update { + copy( + sensorType = event.data.imuType, + status = event.data.status, + completedRestCalibration = event.data.hasCompletedRestCalibration, + magStatus = if (isNew && magStatus == MagnetometerStatus.NOT_SUPPORTED) { + if (event.data.sensorConfig?.magSupported == true) MagnetometerStatus.DISABLED else MagnetometerStatus.NOT_SUPPORTED + } else magStatus, + ) + }) - val magStatus = event.data.sensorConfig?.let { + val remoteMagStatus = event.data.sensorConfig?.let { if (it.magSupported) { if (it.magEnabled) MagnetometerStatus.ENABLED else MagnetometerStatus.DISABLED } else { MagnetometerStatus.NOT_SUPPORTED } - } ?: trackerState?.magStatus ?: MagnetometerStatus.NOT_SUPPORTED + } ?: MagnetometerStatus.NOT_SUPPORTED - val action = TrackerActions.Update { - copy( - sensorType = event.data.imuType, - status = event.data.status, - completedRestCalibration = completedRestCalibration, - magStatus = magStatus, + var desiredMagStatus = tracker.context.state.value.magStatus + val globalMagEnabled = receiver.appContext.config.settings.context.state.value.data.globalMagEnabled + if (remoteMagStatus != desiredMagStatus) { + if (globalMagEnabled && remoteMagStatus != MagnetometerStatus.ENABLED && desiredMagStatus != MagnetometerStatus.NOT_SUPPORTED) + desiredMagStatus = MagnetometerStatus.ENABLED + receiver.context.dispatch( + UDPConnectionActions.SetSensorConfig(sensorId = event.data.sensorId, flags = SensorConfigFlags(magStatus = desiredMagStatus)), ) } - - if (tracker != null) { - tracker.context.dispatch(action) - } else { - val deviceState = device.context.state.value - val mac = deviceState.macAddress ?: run { - AppLogger.udp.warn("[${deviceState.address}] No MAC address available, falling back to IP for hardware ID") - deviceState.address - } - val hardwareId = "$mac:${event.data.sensorId}" - - val existingTracker = receiver.appContext.server.context.state.value.trackers.values - .find { t -> t.context.state.value.deviceId == deviceState.id && t.context.state.value.hardwareId == hardwareId } - - val assignedTracker = if (existingTracker != null) { - receiver.context.dispatch( - UDPConnectionActions.AssignTracker( - trackerId = TrackerIdNum( - id = existingTracker.context.state.value.id, - trackerNum = event.data.sensorId - ), - ), - ) - existingTracker.context.dispatch(action) - existingTracker - } else { - val trackerId = receiver.appContext.server.nextHandle() - val newTracker = Tracker.create( - id = trackerId, - hardwareId = hardwareId, - sensorType = event.data.imuType, - deviceId = deviceState.id, - origin = DeviceOrigin.UDP, - scope = receiver.appContext.server.context.scope, - server = receiver.appContext.server, - settings = receiver.appContext.config.settings, - ) - - receiver.appContext.server.context.dispatch( - VRServerActions.NewTracker(trackerId = trackerId, context = newTracker), - ) - receiver.context.dispatch( - UDPConnectionActions.AssignTracker( - trackerId = TrackerIdNum(id = trackerId, trackerNum = event.data.sensorId), - ), - ) - newTracker.context.dispatch(action) - newTracker - } - - val magStatus = assignedTracker.context.state.value.magStatus - val globalMagEnabled = receiver.appContext.config.settings.context.state.value.data.globalMagEnabled - if (magStatus != MagnetometerStatus.NOT_SUPPORTED && globalMagEnabled) { - receiver.context.dispatch( - UDPConnectionActions.SetSensorConfig( - sensorId = event.data.sensorId, - flags = SensorConfigFlags(magEnabled = magStatus == MagnetometerStatus.ENABLED), - ), - ) - } - - - } } } } @@ -419,9 +409,10 @@ object SensorConfigBehaviour : UDPConnectionBehaviour { SetConfigFlag( sensorId = sensorId, configType = SensorConfigType.MAGNETOMETER, - state = flags.magEnabled + state = flags.magStatus == MagnetometerStatus.ENABLED ) ) + println("SEND CONFIG = ${flags.magStatus}") } } .launchIn(receiver.context.scope) @@ -432,11 +423,13 @@ object AckConfigBehaviour : UDPConnectionBehaviour { override fun observe(receiver: UDPConnection) { receiver.packetEvents.onPacket { event -> val configType = SensorConfigType.fromId(event.data.configType) ?: return@onPacket - if (configType != SensorConfigType.MAGNETOMETER) return@onPacket val flags = receiver.context.state.value.sensorConfigFlags[event.data.sensorId] ?: return@onPacket + val tracker = receiver.getTracker(event.data.sensorId) ?: return@onPacket - val magStatus = if (flags.magEnabled) MagnetometerStatus.ENABLED else MagnetometerStatus.DISABLED - tracker.context.dispatch(TrackerActions.Update { copy(magStatus = magStatus) }) + if (configType == SensorConfigType.MAGNETOMETER) { + tracker.context.dispatch(TrackerActions.Update { copy(magStatus = flags.magStatus) }) + println("UPDATE? = ${flags.magStatus}") + } } } } diff --git a/server/core/src/main/java/dev/slimevr/udp/connection.kt b/server/core/src/main/java/dev/slimevr/udp/connection.kt index 9cfcdc51b..79237586f 100644 --- a/server/core/src/main/java/dev/slimevr/udp/connection.kt +++ b/server/core/src/main/java/dev/slimevr/udp/connection.kt @@ -13,6 +13,7 @@ import kotlinx.coroutines.channels.Channel import kotlinx.coroutines.launch import kotlinx.io.Buffer import kotlinx.io.readByteArray +import solarxr_protocol.datatypes.MagnetometerStatus import java.net.DatagramPacket import java.net.DatagramSocket import java.net.InetAddress @@ -23,7 +24,7 @@ data class LastPing( ) data class SensorConfigFlags( - val magEnabled: Boolean, + val magStatus: MagnetometerStatus, ) data class UDPConnectionState(