Working mag. That was awfull

This commit is contained in:
loucass003 committed 2026-04-21 01:31:24 +02:00
1 parent b7cd179cc6
commit aa505d041b
6 files changed
+120 -138

No files matched your search

@@ -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<ChangeMagToggleRequest> { 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,
))
}
}
@@ -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 {
@@ -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)
}
}
}
@@ -101,7 +101,7 @@ class TrackerRestCheckBehaviour(private val server: VRServer) : TrackingChecklis
private fun computeStep(trackers: List<TrackerState>): 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(),
@@ -229,6 +229,44 @@ object DeviceStatsBehaviour : UDPConnectionBehaviour {
}
object SensorInfoBehaviour : UDPConnectionBehaviour {
private suspend fun assignTracker(receiver: UDPConnection, device: Device, event: PacketEvent<SensorInfo>): Pair<Tracker, Boolean> {
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<SensorInfo> { 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<AckConfigChange> { 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}")
}
}
}
}
@@ -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(