mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Working mag. That was awfull
This commit is contained in:
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(
|
||||
|
||||
Reference in new issue
Block a user