mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-04-06 02:01:58 +02:00
Tracker data
This commit is contained in:
loucass003
@@ -7,7 +7,7 @@ import kotlinx.coroutines.flow.asStateFlow
|
||||
import kotlinx.coroutines.flow.update
|
||||
|
||||
data class BasicModule<S, A>(
|
||||
val reducer: (S, A) -> S,
|
||||
val reducer: ((S, A) -> S)? = null,
|
||||
val observer: ((Context<S, A>) -> Unit)? = null,
|
||||
)
|
||||
|
||||
@@ -21,12 +21,12 @@ data class Context<S, in A>(
|
||||
fun <S, A> createContext(
|
||||
initialState: S,
|
||||
scope: CoroutineScope,
|
||||
reducers: List<(S, A) -> S>,
|
||||
reducers: List<((S, A) -> S)?>,
|
||||
): Context<S, A> {
|
||||
val mutableStateFlow = MutableStateFlow(initialState)
|
||||
|
||||
val applyAction: (S, A) -> S = { currentState, action ->
|
||||
reducers.fold(currentState) { s, reducer -> reducer(s, action) }
|
||||
reducers.filterNotNull().fold(currentState) { s, reducer -> reducer(s, action) }
|
||||
}
|
||||
|
||||
val dispatch: suspend (A) -> Unit = { action ->
|
||||
|
||||
63
server/core/src/main/java/dev/slimevr/skeleton/BodyPart.kt
Normal file
63
server/core/src/main/java/dev/slimevr/skeleton/BodyPart.kt
Normal file
@@ -0,0 +1,63 @@
|
||||
package dev.slimevr.skeleton
|
||||
|
||||
enum class BodyPart(val id: UByte) {
|
||||
HEAD(solarxr_protocol.datatypes.BodyPart.HEAD),
|
||||
NECK(solarxr_protocol.datatypes.BodyPart.NECK),
|
||||
UPPER_CHEST(solarxr_protocol.datatypes.BodyPart.UPPERCHEST),
|
||||
CHEST(solarxr_protocol.datatypes.BodyPart.CHEST),
|
||||
WAIST(solarxr_protocol.datatypes.BodyPart.WAIST),
|
||||
HIP(solarxr_protocol.datatypes.BodyPart.HIP),
|
||||
LEFT_HIP(solarxr_protocol.datatypes.BodyPart.LEFTHIP),
|
||||
RIGHT_HIP(solarxr_protocol.datatypes.BodyPart.RIGHTHIP),
|
||||
LEFT_UPPER_LEG(solarxr_protocol.datatypes.BodyPart.LEFTUPPERLEG),
|
||||
RIGHT_UPPER_LEG(solarxr_protocol.datatypes.BodyPart.RIGHTUPPERLEG),
|
||||
LEFT_LOWER_LEG(solarxr_protocol.datatypes.BodyPart.LEFTLOWERLEG),
|
||||
RIGHT_LOWER_LEG(solarxr_protocol.datatypes.BodyPart.RIGHTLOWERLEG),
|
||||
LEFT_FOOT(solarxr_protocol.datatypes.BodyPart.LEFTFOOT),
|
||||
RIGHT_FOOT(solarxr_protocol.datatypes.BodyPart.RIGHTFOOT),
|
||||
LEFT_FOOT_TRACKER(solarxr_protocol.datatypes.BodyPart.LEFTFOOT),
|
||||
RIGHT_FOOT_TRACKER(solarxr_protocol.datatypes.BodyPart.RIGHTFOOT),
|
||||
LEFT_LOWER_ARM(solarxr_protocol.datatypes.BodyPart.LEFTLOWERARM),
|
||||
RIGHT_LOWER_ARM(solarxr_protocol.datatypes.BodyPart.RIGHTLOWERARM),
|
||||
LEFT_UPPER_ARM(solarxr_protocol.datatypes.BodyPart.LEFTUPPERARM),
|
||||
RIGHT_UPPER_ARM(solarxr_protocol.datatypes.BodyPart.RIGHTUPPERARM),
|
||||
LEFT_SHOULDER(solarxr_protocol.datatypes.BodyPart.LEFTSHOULDER),
|
||||
RIGHT_SHOULDER(solarxr_protocol.datatypes.BodyPart.RIGHTSHOULDER),
|
||||
LEFT_HAND(solarxr_protocol.datatypes.BodyPart.LEFTHAND),
|
||||
RIGHT_HAND(solarxr_protocol.datatypes.BodyPart.RIGHTHAND),
|
||||
LEFT_THUMB_METACARPAL(solarxr_protocol.datatypes.BodyPart.LEFTTHUMBMETACARPAL),
|
||||
LEFT_THUMB_PROXIMAL(solarxr_protocol.datatypes.BodyPart.LEFTTHUMBPROXIMAL),
|
||||
LEFT_THUMB_DISTAL(solarxr_protocol.datatypes.BodyPart.LEFTTHUMBDISTAL),
|
||||
LEFT_INDEX_PROXIMAL(solarxr_protocol.datatypes.BodyPart.LEFTINDEXPROXIMAL),
|
||||
LEFT_INDEX_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.LEFTINDEXINTERMEDIATE),
|
||||
LEFT_INDEX_DISTAL(solarxr_protocol.datatypes.BodyPart.LEFTINDEXDISTAL),
|
||||
LEFT_MIDDLE_PROXIMAL(solarxr_protocol.datatypes.BodyPart.LEFTMIDDLEPROXIMAL),
|
||||
LEFT_MIDDLE_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.LEFTMIDDLEINTERMEDIATE),
|
||||
LEFT_MIDDLE_DISTAL(solarxr_protocol.datatypes.BodyPart.LEFTMIDDLEDISTAL),
|
||||
LEFT_RING_PROXIMAL(solarxr_protocol.datatypes.BodyPart.LEFTRINGPROXIMAL),
|
||||
LEFT_RING_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.LEFTRINGINTERMEDIATE),
|
||||
LEFT_RING_DISTAL(solarxr_protocol.datatypes.BodyPart.LEFTRINGDISTAL),
|
||||
LEFT_LITTLE_PROXIMAL(solarxr_protocol.datatypes.BodyPart.LEFTLITTLEPROXIMAL),
|
||||
LEFT_LITTLE_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.LEFTLITTLEINTERMEDIATE),
|
||||
LEFT_LITTLE_DISTAL(solarxr_protocol.datatypes.BodyPart.LEFTLITTLEDISTAL),
|
||||
RIGHT_THUMB_METACARPAL(solarxr_protocol.datatypes.BodyPart.RIGHTTHUMBMETACARPAL),
|
||||
RIGHT_THUMB_PROXIMAL(solarxr_protocol.datatypes.BodyPart.RIGHTTHUMBPROXIMAL),
|
||||
RIGHT_THUMB_DISTAL(solarxr_protocol.datatypes.BodyPart.RIGHTTHUMBDISTAL),
|
||||
RIGHT_INDEX_PROXIMAL(solarxr_protocol.datatypes.BodyPart.RIGHTINDEXPROXIMAL),
|
||||
RIGHT_INDEX_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.RIGHTINDEXINTERMEDIATE),
|
||||
RIGHT_INDEX_DISTAL(solarxr_protocol.datatypes.BodyPart.RIGHTINDEXDISTAL),
|
||||
RIGHT_MIDDLE_PROXIMAL(solarxr_protocol.datatypes.BodyPart.RIGHTMIDDLEPROXIMAL),
|
||||
RIGHT_MIDDLE_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.RIGHTMIDDLEINTERMEDIATE),
|
||||
RIGHT_MIDDLE_DISTAL(solarxr_protocol.datatypes.BodyPart.RIGHTMIDDLEDISTAL),
|
||||
RIGHT_RING_PROXIMAL(solarxr_protocol.datatypes.BodyPart.RIGHTRINGPROXIMAL),
|
||||
RIGHT_RING_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.RIGHTRINGINTERMEDIATE),
|
||||
RIGHT_RING_DISTAL(solarxr_protocol.datatypes.BodyPart.RIGHTRINGDISTAL),
|
||||
RIGHT_LITTLE_PROXIMAL(solarxr_protocol.datatypes.BodyPart.RIGHTLITTLEPROXIMAL),
|
||||
RIGHT_LITTLE_INTERMEDIATE(solarxr_protocol.datatypes.BodyPart.RIGHTLITTLEINTERMEDIATE),
|
||||
RIGHT_LITTLE_DISTAL(solarxr_protocol.datatypes.BodyPart.RIGHTLITTLEDISTAL);
|
||||
|
||||
companion object {
|
||||
private val map = entries.associateBy { it.id }
|
||||
fun fromId(id: UByte) = map[id]
|
||||
}
|
||||
}
|
||||
@@ -21,19 +21,26 @@ data class DeviceState(
|
||||
val id: Int,
|
||||
val name: String,
|
||||
val address: String,
|
||||
val batteryLevel: Int,
|
||||
val batteryVoltage: Int,
|
||||
val batteryLevel: Float,
|
||||
val batteryVoltage: Float,
|
||||
val ping: Long?,
|
||||
val signalStrength: Int?,
|
||||
val origin: DeviceOrigin,
|
||||
)
|
||||
|
||||
sealed interface DeviceActions {
|
||||
data class SetBattery(val level: Int, val voltage: Int) : DeviceActions
|
||||
data class SetPing(val ping: Long) : DeviceActions
|
||||
data class SetSignalStrength(val signalStrength: Int) : DeviceActions
|
||||
data class Update(val transform: DeviceState.() -> DeviceState) : DeviceActions
|
||||
}
|
||||
|
||||
val DeviceStatsModule = DeviceModule(
|
||||
reducer = { s, a -> if (a is DeviceActions.Update) a.transform(s) else s },
|
||||
observer = {
|
||||
it.state.onEach {
|
||||
println("Device state changed $it")
|
||||
}.launchIn(it.scope)
|
||||
}
|
||||
)
|
||||
|
||||
typealias DeviceContext = Context<DeviceState, DeviceActions>
|
||||
typealias DeviceModule = BasicModule<DeviceState, DeviceActions>
|
||||
|
||||
@@ -41,36 +48,19 @@ data class Device(
|
||||
val context: DeviceContext,
|
||||
)
|
||||
|
||||
val PingModule = DeviceModule(
|
||||
reducer = { s, a ->
|
||||
when (a) {
|
||||
is DeviceActions.SetPing -> s.copy(ping = a.ping)
|
||||
else -> s
|
||||
}
|
||||
},
|
||||
observer = {
|
||||
it.state
|
||||
.distinctUntilChangedBy { device -> device.ping }
|
||||
.filter { device -> device.ping != null }
|
||||
.onEach { device ->
|
||||
println("[${device.name}] ping change to ${device.ping}")
|
||||
}.launchIn(it.scope)
|
||||
},
|
||||
)
|
||||
|
||||
fun createDevice(scope: CoroutineScope, id: Int, address: String, origin: DeviceOrigin, serverContext: VRServer): Device {
|
||||
val deviceState = DeviceState(
|
||||
id = id,
|
||||
name = "Device $id",
|
||||
batteryLevel = 0,
|
||||
batteryVoltage = 0,
|
||||
batteryLevel = 0f,
|
||||
batteryVoltage = 0f,
|
||||
origin = origin,
|
||||
address = address,
|
||||
ping = null,
|
||||
signalStrength = null,
|
||||
)
|
||||
|
||||
val modules = listOf(PingModule)
|
||||
val modules = listOf(DeviceStatsModule)
|
||||
|
||||
val context = createContext(
|
||||
initialState = deviceState,
|
||||
|
||||
30
server/core/src/main/java/dev/slimevr/tracker/imu.kt
Normal file
30
server/core/src/main/java/dev/slimevr/tracker/imu.kt
Normal file
@@ -0,0 +1,30 @@
|
||||
package dev.slimevr.tracker
|
||||
|
||||
enum class IMUType(val id: UByte) {
|
||||
UNKNOWN(solarxr_protocol.datatypes.hardware_info.ImuType.Other.toUByte()),
|
||||
MPU9250(solarxr_protocol.datatypes.hardware_info.ImuType.MPU9250.toUByte()),
|
||||
MPU6500(solarxr_protocol.datatypes.hardware_info.ImuType.MPU6500.toUByte()),
|
||||
BNO080(solarxr_protocol.datatypes.hardware_info.ImuType.BNO080.toUByte()),
|
||||
BNO085(solarxr_protocol.datatypes.hardware_info.ImuType.BNO085.toUByte()),
|
||||
BNO055(solarxr_protocol.datatypes.hardware_info.ImuType.BNO055.toUByte()),
|
||||
MPU6050(solarxr_protocol.datatypes.hardware_info.ImuType.MPU6050.toUByte()),
|
||||
BNO086(solarxr_protocol.datatypes.hardware_info.ImuType.BNO086.toUByte()),
|
||||
BMI160(solarxr_protocol.datatypes.hardware_info.ImuType.BMI160.toUByte()),
|
||||
ICM20948(solarxr_protocol.datatypes.hardware_info.ImuType.ICM20948.toUByte()),
|
||||
ICM42688(solarxr_protocol.datatypes.hardware_info.ImuType.ICM42688.toUByte()),
|
||||
BMI270(solarxr_protocol.datatypes.hardware_info.ImuType.BMI270.toUByte()),
|
||||
LSM6DS3TRC(solarxr_protocol.datatypes.hardware_info.ImuType.LSM6DS3TRC.toUByte()),
|
||||
LSM6DSV(solarxr_protocol.datatypes.hardware_info.ImuType.LSM6DSV.toUByte()),
|
||||
LSM6DSO(solarxr_protocol.datatypes.hardware_info.ImuType.LSM6DSO.toUByte()),
|
||||
LSM6DSR(solarxr_protocol.datatypes.hardware_info.ImuType.LSM6DSR.toUByte()),
|
||||
ICM45686(solarxr_protocol.datatypes.hardware_info.ImuType.ICM45686.toUByte()),
|
||||
ICM45605(solarxr_protocol.datatypes.hardware_info.ImuType.ICM45605.toUByte()),
|
||||
ADC_RESISTANCE(solarxr_protocol.datatypes.hardware_info.ImuType.ADCRESISTANCE.toUByte()),
|
||||
DEV_RESERVED(solarxr_protocol.datatypes.hardware_info.ImuType.DEVRESERVED.toUByte()),
|
||||
;
|
||||
|
||||
companion object {
|
||||
private val map = entries.associateBy { it.id }
|
||||
fun fromId(id: UByte) = map[id]
|
||||
}
|
||||
}
|
||||
@@ -1,18 +1,96 @@
|
||||
package dev.slimevr.tracker
|
||||
|
||||
import dev.slimevr.VRServer
|
||||
import dev.slimevr.context.BasicModule
|
||||
import dev.slimevr.context.Context
|
||||
import dev.slimevr.context.createContext
|
||||
import dev.slimevr.skeleton.BodyPart
|
||||
import io.github.axisangles.ktmath.Quaternion
|
||||
import kotlinx.coroutines.CoroutineScope
|
||||
import kotlinx.coroutines.flow.launchIn
|
||||
import kotlinx.coroutines.flow.map
|
||||
import kotlinx.coroutines.flow.onEach
|
||||
|
||||
enum class TrackerStatus(val id: UByte) {
|
||||
DISCONNECTED(solarxr_protocol.datatypes.TrackerStatus.DISCONNECTED),
|
||||
OK(solarxr_protocol.datatypes.TrackerStatus.OK),
|
||||
BUSY(solarxr_protocol.datatypes.TrackerStatus.BUSY),
|
||||
ERROR(solarxr_protocol.datatypes.TrackerStatus.ERROR),
|
||||
OCCLUDED(solarxr_protocol.datatypes.TrackerStatus.OCCLUDED),
|
||||
TIMEDOUT(solarxr_protocol.datatypes.TrackerStatus.TIMEDOUT);
|
||||
|
||||
companion object {
|
||||
private val map = entries.associateBy { it.id }
|
||||
fun fromId(id: UByte) = map[id]
|
||||
}
|
||||
}
|
||||
|
||||
data class TrackerIdNum(val id: Int, val trackerNum: Int)
|
||||
|
||||
data class TrackerState(
|
||||
val id: Int,
|
||||
val name: String,
|
||||
val hardwareId: String,
|
||||
val sensorType: IMUType,
|
||||
val bodyPart: BodyPart?,
|
||||
val status: TrackerStatus,
|
||||
val customName: String?,
|
||||
val rawRotation: Quaternion,
|
||||
val deviceId: Int,
|
||||
val origin: DeviceOrigin
|
||||
)
|
||||
|
||||
sealed interface TrackerActions {
|
||||
data class UpdateRotation(val rotation: Quaternion)
|
||||
data class Update(val transform: TrackerState.() -> TrackerState) : TrackerActions
|
||||
}
|
||||
|
||||
typealias TrackerContext = Context<TrackerState, TrackerActions>
|
||||
typealias TrackerModule = BasicModule<TrackerState, TrackerActions>
|
||||
|
||||
data class Tracker(
|
||||
val context: TrackerContext
|
||||
)
|
||||
|
||||
val TrackerInfosModule = TrackerModule(
|
||||
reducer = { s, a -> if (a is TrackerActions.Update) a.transform(s) else s },
|
||||
observer = {
|
||||
it.state.onEach { println("Tracker $it") }.launchIn(it.scope)
|
||||
}
|
||||
)
|
||||
|
||||
fun createTracker(
|
||||
scope: CoroutineScope,
|
||||
id: Int,
|
||||
deviceId: Int,
|
||||
sensorType: IMUType,
|
||||
hardwareId: String,
|
||||
origin: DeviceOrigin,
|
||||
serverContext: VRServer
|
||||
): Tracker {
|
||||
val trackerState = TrackerState(
|
||||
id = id,
|
||||
hardwareId = hardwareId,
|
||||
name = "Tracker #$id",
|
||||
rawRotation = Quaternion.IDENTITY,
|
||||
status = TrackerStatus.DISCONNECTED,
|
||||
bodyPart = null,
|
||||
origin = origin,
|
||||
deviceId = deviceId,
|
||||
customName = null,
|
||||
sensorType = sensorType
|
||||
)
|
||||
|
||||
val modules = listOf(TrackerInfosModule)
|
||||
|
||||
val context = createContext(
|
||||
initialState = trackerState,
|
||||
reducers = modules.map { it.reducer },
|
||||
scope = scope,
|
||||
)
|
||||
|
||||
modules.map { it.observer }.forEach { it?.invoke(context) }
|
||||
|
||||
return Tracker(
|
||||
context = context,
|
||||
)
|
||||
}
|
||||
|
||||
@@ -4,9 +4,14 @@ import dev.slimevr.VRServer
|
||||
import dev.slimevr.VRServerActions
|
||||
import dev.slimevr.context.Context
|
||||
import dev.slimevr.context.createContext
|
||||
import dev.slimevr.tracker.Device
|
||||
import dev.slimevr.tracker.DeviceActions
|
||||
import dev.slimevr.tracker.DeviceOrigin
|
||||
import dev.slimevr.tracker.Tracker
|
||||
import dev.slimevr.tracker.TrackerActions
|
||||
import dev.slimevr.tracker.TrackerIdNum
|
||||
import dev.slimevr.tracker.createDevice
|
||||
import dev.slimevr.tracker.createTracker
|
||||
import io.ktor.network.sockets.BoundDatagramSocket
|
||||
import io.ktor.network.sockets.Datagram
|
||||
import io.ktor.network.sockets.InetSocketAddress
|
||||
@@ -31,13 +36,16 @@ data class UDPConnectionState(
|
||||
val address: String,
|
||||
val port: Int,
|
||||
val deviceId: Int?,
|
||||
val trackerIds: List<TrackerIdNum>
|
||||
)
|
||||
|
||||
sealed interface UDPConnectionActions {
|
||||
data class StartPing(val startTime: Long) : UDPConnectionActions
|
||||
data class ReceivedPong(val id: Int, val duration: Long) : UDPConnectionActions
|
||||
data class Handshake(val deviceId: Int) : UDPConnectionActions
|
||||
data class LastPacket(val packetNum: Long? = null, val time: Long) : UDPConnectionActions
|
||||
data class LastPacket(val packetNum: Long? = null, val time: Long) :
|
||||
UDPConnectionActions
|
||||
data class AssignTracker(val trackerId: TrackerIdNum) : UDPConnectionActions
|
||||
}
|
||||
|
||||
typealias UDPConnectionContext = Context<UDPConnectionState, UDPConnectionActions>
|
||||
@@ -47,10 +55,12 @@ data class UDPConnection(
|
||||
val serverContext: VRServer,
|
||||
val packetEvents: PacketDispatcher,
|
||||
val send: (Packet) -> Unit,
|
||||
val getDevice: () -> Device?,
|
||||
val getTracker: (sensorId: Int) -> Tracker?,
|
||||
)
|
||||
|
||||
data class UDPConnectionModule(
|
||||
val reducer: (UDPConnectionState, UDPConnectionActions) -> UDPConnectionState,
|
||||
val reducer: ((UDPConnectionState, UDPConnectionActions) -> UDPConnectionState)? = null,
|
||||
val observer: ((UDPConnection) -> Unit)? = null,
|
||||
)
|
||||
|
||||
@@ -77,7 +87,12 @@ val PacketModule = UDPConnectionModule(
|
||||
val now = System.currentTimeMillis()
|
||||
if (now - state.lastPacket > 5000 && packet.packetNumber == 0L) {
|
||||
it.context.scope.launch {
|
||||
it.context.dispatch(UDPConnectionActions.LastPacket(packetNum = 0, time = now))
|
||||
it.context.dispatch(
|
||||
UDPConnectionActions.LastPacket(
|
||||
packetNum = 0,
|
||||
time = now
|
||||
)
|
||||
)
|
||||
println("Reconnecting")
|
||||
}
|
||||
} else if (packet.packetNumber < state.lastPacketNum) {
|
||||
@@ -125,17 +140,24 @@ val PingModule = UDPConnectionModule(
|
||||
val deviceId = state.deviceId ?: return@on
|
||||
|
||||
if (paket.data.pingId != state.lastPing.id + 1) {
|
||||
println("Ping ID does not match, ignoring")
|
||||
println("Ping ID does not match, ignoring ${paket.data.pingId} != ${state.lastPing.id + 1}")
|
||||
return@on
|
||||
}
|
||||
|
||||
val ping = System.currentTimeMillis() - state.lastPing.startTime
|
||||
|
||||
val device = it.serverContext.getDeviceContext(deviceId) ?: return@on
|
||||
val device = it.serverContext.getDevice(deviceId) ?: return@on
|
||||
|
||||
it.context.scope.launch {
|
||||
it.context.dispatch(UDPConnectionActions.ReceivedPong(id = paket.data.pingId, duration = ping))
|
||||
device.context.dispatch(DeviceActions.SetPing(ping))
|
||||
it.context.dispatch(
|
||||
UDPConnectionActions.ReceivedPong(
|
||||
id = paket.data.pingId,
|
||||
duration = ping
|
||||
)
|
||||
)
|
||||
device.context.dispatch(DeviceActions.Update {
|
||||
copy(ping = ping)
|
||||
})
|
||||
}
|
||||
}
|
||||
},
|
||||
@@ -144,7 +166,11 @@ val PingModule = UDPConnectionModule(
|
||||
val HandshakeModule = UDPConnectionModule(
|
||||
reducer = { s, a ->
|
||||
when (a) {
|
||||
is UDPConnectionActions.Handshake -> s.copy(didHandshake = true, deviceId = a.deviceId)
|
||||
is UDPConnectionActions.Handshake -> s.copy(
|
||||
didHandshake = true,
|
||||
deviceId = a.deviceId
|
||||
)
|
||||
|
||||
else -> s
|
||||
}
|
||||
},
|
||||
@@ -158,23 +184,133 @@ val HandshakeModule = UDPConnectionModule(
|
||||
val newDevice = createDevice(
|
||||
id = deviceId,
|
||||
scope = it.serverContext.context.scope,
|
||||
address = packet.data.mac ?: error("no mac address?"),
|
||||
address = packet.data.macString ?: error("no mac address?"),
|
||||
origin = DeviceOrigin.UDP,
|
||||
serverContext = it.serverContext,
|
||||
)
|
||||
|
||||
it.context.scope.launch {
|
||||
it.serverContext.context.dispatch(VRServerActions.NewDevice(deviceId = deviceId, context = newDevice))
|
||||
it.serverContext.context.dispatch(
|
||||
VRServerActions.NewDevice(
|
||||
deviceId = deviceId,
|
||||
context = newDevice
|
||||
)
|
||||
)
|
||||
it.context.dispatch(UDPConnectionActions.Handshake(deviceId))
|
||||
it.send(HandshakeResponse())
|
||||
it.send(Handshake())
|
||||
}
|
||||
} else {
|
||||
it.send(HandshakeResponse())
|
||||
it.send(Handshake())
|
||||
}
|
||||
}
|
||||
},
|
||||
)
|
||||
|
||||
val DeviceStatsModule = UDPConnectionModule(
|
||||
observer = {
|
||||
it.packetEvents.on<BatteryLevel> { event ->
|
||||
val device = it.getDevice() ?: return@on
|
||||
|
||||
device.context.scope.launch {
|
||||
device.context.dispatch(DeviceActions.Update {
|
||||
copy(
|
||||
batteryLevel = event.data.level,
|
||||
batteryVoltage = event.data.voltage
|
||||
)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
it.packetEvents.on<SignalStrength> { event ->
|
||||
val device = it.getDevice() ?: return@on
|
||||
|
||||
device.context.scope.launch {
|
||||
device.context.dispatch(DeviceActions.Update {
|
||||
copy(signalStrength = event.data.signal)
|
||||
})
|
||||
}
|
||||
}
|
||||
}
|
||||
)
|
||||
|
||||
val SensorInfoModule = UDPConnectionModule(
|
||||
reducer = { s, a ->
|
||||
when (a) {
|
||||
is UDPConnectionActions.AssignTracker -> {
|
||||
s.copy(trackerIds = s.trackerIds + a.trackerId)
|
||||
}
|
||||
|
||||
else -> s
|
||||
}
|
||||
},
|
||||
observer = {
|
||||
it.packetEvents.on<SensorInfo> { event ->
|
||||
val tracker = it.getTracker(event.data.sensorId)
|
||||
|
||||
val action = TrackerActions.Update {
|
||||
copy(
|
||||
sensorType = event.data.imuType,
|
||||
status = event.data.status,
|
||||
)
|
||||
}
|
||||
|
||||
if (tracker != null) {
|
||||
tracker.context.scope.launch {
|
||||
tracker.context.dispatch(action)
|
||||
}
|
||||
} else {
|
||||
val device = it.getDevice()
|
||||
?: error("invalid state - a device should exist at this point")
|
||||
val deviceState = device.context.state.value
|
||||
val trackerId = it.serverContext.nextHandle()
|
||||
val newTracker = createTracker(
|
||||
id = trackerId,
|
||||
hardwareId = "${deviceState.address}:${event.data.sensorId}",
|
||||
sensorType = event.data.imuType,
|
||||
deviceId = deviceState.id,
|
||||
origin = DeviceOrigin.UDP,
|
||||
serverContext = it.serverContext,
|
||||
scope = it.serverContext.context.scope
|
||||
)
|
||||
|
||||
it.serverContext.context.scope.launch {
|
||||
it.serverContext.context.dispatch(
|
||||
VRServerActions.NewTracker(
|
||||
trackerId = trackerId,
|
||||
context = newTracker
|
||||
)
|
||||
)
|
||||
it.context.dispatch(
|
||||
UDPConnectionActions.AssignTracker(
|
||||
trackerId = TrackerIdNum(
|
||||
id = trackerId,
|
||||
trackerNum = event.data.sensorId
|
||||
)
|
||||
)
|
||||
)
|
||||
newTracker.context.dispatch(action)
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
)
|
||||
|
||||
val SensorRotationModule = UDPConnectionModule(
|
||||
observer = { context ->
|
||||
context.packetEvents.on<RotationData> { event ->
|
||||
val tracker = context.getTracker(event.data.sensorId) ?: return@on
|
||||
tracker.context.scope.launch {
|
||||
tracker.context.dispatch(
|
||||
TrackerActions.Update {
|
||||
copy(rawRotation = event.data.rotation)
|
||||
}
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
)
|
||||
|
||||
fun createUDPConnectionContext(
|
||||
id: String,
|
||||
socket: BoundDatagramSocket,
|
||||
@@ -182,7 +318,14 @@ fun createUDPConnectionContext(
|
||||
serverContext: VRServer,
|
||||
scope: CoroutineScope,
|
||||
): UDPConnection {
|
||||
val modules = listOf(PacketModule, HandshakeModule, PingModule)
|
||||
val modules = listOf(
|
||||
PacketModule,
|
||||
HandshakeModule,
|
||||
PingModule,
|
||||
DeviceStatsModule,
|
||||
SensorInfoModule,
|
||||
SensorRotationModule
|
||||
)
|
||||
|
||||
val context = createContext(
|
||||
initialState = UDPConnectionState(
|
||||
@@ -194,6 +337,7 @@ fun createUDPConnectionContext(
|
||||
address = remoteAddress.hostname,
|
||||
port = remoteAddress.port,
|
||||
deviceId = null,
|
||||
trackerIds = listOf()
|
||||
),
|
||||
reducers = modules.map { it.reducer },
|
||||
scope = scope,
|
||||
@@ -201,23 +345,28 @@ fun createUDPConnectionContext(
|
||||
|
||||
val dispatcher = PacketDispatcher()
|
||||
|
||||
val sendFunc = { packet: Packet ->
|
||||
scope.launch {
|
||||
val packetNum = context.state.value.lastPacketNum + 1
|
||||
|
||||
val bytePacket = buildPacket {
|
||||
writePacket(this, packet, packetNum)
|
||||
}
|
||||
socket.send(Datagram(bytePacket, remoteAddress))
|
||||
}
|
||||
Unit
|
||||
}
|
||||
|
||||
val conn = UDPConnection(
|
||||
context = context,
|
||||
serverContext = serverContext,
|
||||
dispatcher,
|
||||
send = sendFunc,
|
||||
send = { packet: Packet ->
|
||||
scope.launch {
|
||||
val bytePacket = buildPacket {
|
||||
writePacket(this, packet)
|
||||
}
|
||||
socket.send(Datagram(bytePacket, remoteAddress))
|
||||
}
|
||||
},
|
||||
getDevice = {
|
||||
val deviceId = context.state.value.deviceId
|
||||
if (deviceId != null) serverContext.getDevice(deviceId)
|
||||
else null
|
||||
},
|
||||
getTracker = { id ->
|
||||
val trackerId = context.state.value.trackerIds.find { it.trackerNum == id }
|
||||
if (trackerId != null) serverContext.getTracker(trackerId.id)
|
||||
else null
|
||||
}
|
||||
)
|
||||
|
||||
modules.map { it.observer }.forEach { it?.invoke(conn) }
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
package dev.slimevr.tracker.udp
|
||||
|
||||
import dev.slimevr.tracker.IMUType
|
||||
import dev.slimevr.tracker.TrackerStatus
|
||||
import io.github.axisangles.ktmath.Quaternion
|
||||
import io.github.axisangles.ktmath.Vector3
|
||||
import io.ktor.utils.io.core.remaining
|
||||
@@ -7,10 +9,28 @@ import kotlinx.io.Sink
|
||||
import kotlinx.io.Source
|
||||
import kotlinx.io.readByteArray
|
||||
import kotlinx.io.readFloat
|
||||
import kotlinx.io.readString
|
||||
import kotlinx.io.readUByte
|
||||
import kotlinx.io.writeFloat
|
||||
import kotlinx.io.writeUByte
|
||||
import kotlin.reflect.KClass
|
||||
|
||||
// ── Enums ───────────────────────────────────────────────────────────────────
|
||||
private fun Source.readU8(): Int = readByte().toInt() and 0xFF
|
||||
|
||||
private fun Source.readSafeFloat(): Float = readFloat().let { if (it.isNaN()) 0f else it }
|
||||
|
||||
private fun Source.readSafeQuat(): Quaternion {
|
||||
val x = readFloat()
|
||||
val y = readFloat()
|
||||
val z = readFloat()
|
||||
val w = readFloat()
|
||||
return if (x.isNaN() || y.isNaN() || z.isNaN() || w.isNaN() || (x == 0f && y == 0f && z == 0f && w == 0f)) {
|
||||
Quaternion.IDENTITY
|
||||
} else {
|
||||
Quaternion(w, x, y, z)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
enum class PacketType(val id: Int) {
|
||||
HEARTBEAT(0),
|
||||
@@ -29,8 +49,13 @@ enum class PacketType(val id: Int) {
|
||||
SIGNAL_STRENGTH(19),
|
||||
TEMPERATURE(20),
|
||||
USER_ACTION(21),
|
||||
PROTOCOL_CHANGE(200),
|
||||
;
|
||||
FEATURE_FLAGS(22),
|
||||
ROTATION_AND_ACCEL(23),
|
||||
ACK_CONFIG_CHANGE(24),
|
||||
SET_CONFIG_FLAG(25),
|
||||
FLEX_DATA(26),
|
||||
POSITION(27),
|
||||
PROTOCOL_CHANGE(200);
|
||||
|
||||
companion object {
|
||||
private val map = entries.associateBy { it.id }
|
||||
@@ -38,229 +63,255 @@ enum class PacketType(val id: Int) {
|
||||
}
|
||||
}
|
||||
|
||||
enum class UserAction(val id: Int) {
|
||||
RESET_FULL(2),
|
||||
RESET_YAW(3),
|
||||
RESET_MOUNTING(4),
|
||||
PAUSE_TRACKING(5),
|
||||
;
|
||||
|
||||
companion object {
|
||||
private val map = entries.associateBy { it.id }
|
||||
fun fromId(id: Int) = map[id]
|
||||
}
|
||||
sealed interface Packet {
|
||||
fun write(dst: Sink) {}
|
||||
}
|
||||
|
||||
sealed interface Packet
|
||||
sealed interface SensorSpecificPacket : Packet {
|
||||
val sensorId: Int
|
||||
}
|
||||
sealed interface RotationPacket : SensorSpecificPacket {
|
||||
val rotation: Quaternion
|
||||
}
|
||||
|
||||
data object Heartbeat : Packet
|
||||
data class Handshake(val board: Int = 0, val imu: Int = 0, val mcu: Int = 0, val pVer: Int = 0, val firmware: String? = null, val mac: String? = null) : Packet
|
||||
data class HandshakeResponse(val nothing: Unit = Unit) : Packet
|
||||
data class Rotation(override val sensorId: Int = 0, override val rotation: Quaternion = Quaternion.IDENTITY) : RotationPacket
|
||||
data class Accel(val acceleration: Vector3 = Vector3.NULL, override val sensorId: Int = 0) : SensorSpecificPacket
|
||||
data class PingPong(val pingId: Int = 0) : Packet
|
||||
data class Serial(val serial: String = "") : Packet
|
||||
data class BatteryLevel(val voltage: Float = 0f, val level: Float = 0f) : Packet
|
||||
data class Tap(override val sensorId: Int = 0, val tap: Int = 0) : SensorSpecificPacket
|
||||
data class Error(override val sensorId: Int = 0, val errorNumber: Int = 0) : SensorSpecificPacket
|
||||
data class SensorInfo(override val sensorId: Int = 0, val status: Int = 0, val type: Int = 0) : SensorSpecificPacket
|
||||
data class Rotation2(override val sensorId: Int = 1, override val rotation: Quaternion = Quaternion.IDENTITY) : RotationPacket
|
||||
data class RotationData(override val sensorId: Int = 0, val dataType: Int = 0, val rotation: Quaternion = Quaternion.IDENTITY, val calibration: Int = 0) : SensorSpecificPacket
|
||||
data class MagnetometerAccuracy(override val sensorId: Int = 0, val accuracy: Float = 0f) : SensorSpecificPacket
|
||||
data class SignalStrength(override val sensorId: Int = 0, val signal: Int = 0) : SensorSpecificPacket
|
||||
data class Temperature(override val sensorId: Int = 0, val temp: Float = 0f) : SensorSpecificPacket
|
||||
data class UserActionPacket(val action: UserAction? = null) : Packet
|
||||
data class ProtocolChange(val targetProtocol: Int = 0, val targetVersion: Int = 0) : Packet
|
||||
|
||||
private fun Source.readU8() = readByte().toInt() and 0xFF
|
||||
private fun Sink.writeU8(v: Int) = writeByte(v.toByte())
|
||||
data class Handshake(
|
||||
val boardType: Int = 0,
|
||||
val imuType: Int = 0,
|
||||
val mcuType: Int = 0,
|
||||
val protocolVersion: Int = 0,
|
||||
val firmware: String? = null,
|
||||
val macString: String? = null
|
||||
) : Packet {
|
||||
override fun write(dst: Sink) {
|
||||
dst.writeByte(PacketType.HANDSHAKE.id.toByte())
|
||||
dst.write("Hey OVR =D 5".toByteArray(Charsets.US_ASCII))
|
||||
}
|
||||
|
||||
private fun Source.readSafeFloat() = readFloat().let { if (it.isNaN()) 0f else it }
|
||||
private fun Source.readSafeQuat() = Quaternion(readFloat(), readFloat(), readFloat(), readFloat()).let {
|
||||
if (it.x.isNaN() || (it.x == 0f && it.y == 0f && it.z == 0f && it.w == 0f)) Quaternion.IDENTITY else it
|
||||
}
|
||||
|
||||
private fun Sink.writeQuat(q: Quaternion) {
|
||||
writeFloat(q.x)
|
||||
writeFloat(q.y)
|
||||
writeFloat(q.z)
|
||||
writeFloat(q.w)
|
||||
}
|
||||
|
||||
private fun Source.readStr(len: Int) = buildString {
|
||||
repeat(len) { readByte().toInt().takeIf { it != 0 }?.let { append(it.toChar()) } }
|
||||
}
|
||||
|
||||
object PacketCodec {
|
||||
fun read(type: PacketType, src: Source): Packet = when (type) {
|
||||
PacketType.HEARTBEAT -> Heartbeat
|
||||
|
||||
PacketType.HANDSHAKE -> with(src) {
|
||||
if (exhausted()) return Handshake()
|
||||
companion object {
|
||||
fun read(src: Source): Handshake = with(src) {
|
||||
if (remaining == 0L) return Handshake()
|
||||
val b = if (remaining >= 4) readInt() else 0
|
||||
val i = if (remaining >= 4) readInt() else 0
|
||||
val m = if (remaining >= 4) readInt() else 0
|
||||
if (remaining >= 12) {
|
||||
readInt()
|
||||
readInt()
|
||||
readInt()
|
||||
}
|
||||
if (remaining >= 12) { readInt(); readInt(); readInt() }
|
||||
val p = if (remaining >= 4) readInt() else 0
|
||||
val f = if (remaining >= 1) readStr(readByte().toInt()) else null
|
||||
val mac = if (remaining >= 6) readByteArray(6).joinToString(":") { "%02X".format(it) }.takeIf { it != "00:00:00:00:00:00" } else null
|
||||
val f = if (remaining >= 1) readString(readByte().toLong()) else null
|
||||
val mac = if (remaining >= 6) {
|
||||
val bytes = readByteArray(6)
|
||||
bytes.joinToString(":") { "%02X".format(it) }.takeIf { it != "00:00:00:00:00:00" }
|
||||
} else null
|
||||
Handshake(b, i, m, p, f, mac)
|
||||
}
|
||||
|
||||
PacketType.ROTATION -> Rotation(rotation = src.readSafeQuat())
|
||||
|
||||
PacketType.ACCEL -> Accel(
|
||||
Vector3(src.readSafeFloat(), src.readSafeFloat(), src.readSafeFloat()),
|
||||
if (!src.exhausted()) src.readU8() else 0,
|
||||
)
|
||||
|
||||
PacketType.PING_PONG -> PingPong(src.readInt())
|
||||
|
||||
PacketType.SERIAL -> Serial(src.readStr(src.readInt()))
|
||||
|
||||
PacketType.BATTERY_LEVEL -> src.readSafeFloat().let { f ->
|
||||
if (src.remaining >= 4) BatteryLevel(f, src.readSafeFloat()) else BatteryLevel(0f, f)
|
||||
}
|
||||
|
||||
PacketType.TAP -> Tap(src.readU8(), src.readU8())
|
||||
|
||||
PacketType.ERROR -> Error(src.readU8(), src.readU8())
|
||||
|
||||
PacketType.SENSOR_INFO -> SensorInfo(src.readU8(), src.readU8(), if (!src.exhausted()) src.readU8() else 0)
|
||||
|
||||
PacketType.ROTATION_2 -> Rotation2(rotation = src.readSafeQuat())
|
||||
|
||||
PacketType.ROTATION_DATA -> RotationData(src.readU8(), src.readU8(), src.readSafeQuat(), src.readU8())
|
||||
|
||||
PacketType.MAGNETOMETER_ACCURACY -> MagnetometerAccuracy(src.readU8(), src.readSafeFloat())
|
||||
|
||||
PacketType.SIGNAL_STRENGTH -> SignalStrength(src.readU8(), src.readByte().toInt())
|
||||
|
||||
PacketType.TEMPERATURE -> Temperature(src.readU8(), src.readSafeFloat())
|
||||
|
||||
PacketType.USER_ACTION -> UserActionPacket(UserAction.fromId(src.readU8()))
|
||||
|
||||
PacketType.PROTOCOL_CHANGE -> ProtocolChange(src.readU8(), src.readU8())
|
||||
}
|
||||
|
||||
fun write(dst: Sink, packet: Packet) = when (packet) {
|
||||
is Heartbeat -> {}
|
||||
|
||||
is HandshakeResponse -> {
|
||||
dst.writeU8(PacketType.HANDSHAKE.id)
|
||||
dst.write("Hey OVR =D 5".toByteArray(Charsets.US_ASCII))
|
||||
}
|
||||
|
||||
is Rotation -> dst.writeQuat(packet.rotation)
|
||||
|
||||
is Accel -> {
|
||||
dst.writeFloat(packet.acceleration.x)
|
||||
dst.writeFloat(packet.acceleration.y)
|
||||
dst.writeFloat(packet.acceleration.z)
|
||||
}
|
||||
|
||||
is PingPong -> dst.writeInt(packet.pingId)
|
||||
|
||||
is Serial -> {
|
||||
dst.writeInt(packet.serial.length)
|
||||
packet.serial.forEach { dst.writeU8(it.code) }
|
||||
}
|
||||
|
||||
is BatteryLevel -> {
|
||||
dst.writeFloat(packet.voltage)
|
||||
dst.writeFloat(packet.level)
|
||||
}
|
||||
|
||||
is Tap -> {
|
||||
dst.writeU8(packet.sensorId)
|
||||
dst.writeU8(packet.tap)
|
||||
}
|
||||
|
||||
is Error -> {
|
||||
dst.writeU8(packet.sensorId)
|
||||
dst.writeU8(packet.errorNumber)
|
||||
}
|
||||
|
||||
is SensorInfo -> {
|
||||
dst.writeU8(packet.sensorId)
|
||||
dst.writeU8(packet.status)
|
||||
dst.writeU8(packet.type)
|
||||
}
|
||||
|
||||
is Rotation2 -> dst.writeQuat(packet.rotation)
|
||||
|
||||
is RotationData -> {
|
||||
dst.writeU8(packet.sensorId)
|
||||
dst.writeU8(packet.dataType)
|
||||
dst.writeQuat(packet.rotation)
|
||||
dst.writeU8(packet.calibration)
|
||||
}
|
||||
|
||||
is MagnetometerAccuracy -> {
|
||||
dst.writeU8(packet.sensorId)
|
||||
dst.writeFloat(packet.accuracy)
|
||||
}
|
||||
|
||||
is SignalStrength -> {
|
||||
dst.writeU8(packet.sensorId)
|
||||
dst.writeByte(packet.signal.toByte())
|
||||
}
|
||||
|
||||
is Temperature -> {
|
||||
dst.writeU8(packet.sensorId)
|
||||
dst.writeFloat(packet.temp)
|
||||
}
|
||||
|
||||
is UserActionPacket -> dst.writeU8(packet.action?.id ?: 0)
|
||||
|
||||
is ProtocolChange -> {
|
||||
dst.writeU8(packet.targetProtocol)
|
||||
dst.writeU8(packet.targetVersion)
|
||||
}
|
||||
|
||||
else -> error("unhandled packet")
|
||||
}
|
||||
}
|
||||
|
||||
// ── Entry Points ────────────────────────────────────────────────────────────
|
||||
data class Rotation(override val sensorId: Int = 0, val rotation: Quaternion = Quaternion.IDENTITY) : SensorSpecificPacket {
|
||||
companion object { fun read(src: Source) = Rotation(0, src.readSafeQuat()) }
|
||||
}
|
||||
|
||||
fun writePacket(dst: Sink, packet: Packet, packetNum: Long) {
|
||||
data class Accel(val acceleration: Vector3 = Vector3.NULL, override val sensorId: Int = 0) : SensorSpecificPacket {
|
||||
companion object {
|
||||
fun read(src: Source) = Accel(
|
||||
Vector3(src.readSafeFloat(), src.readSafeFloat(), src.readSafeFloat()),
|
||||
if (src.remaining > 0) src.readU8() else 0
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
data class PingPong(val pingId: Int = 0) : Packet {
|
||||
override fun write(dst: Sink) { dst.writeInt(pingId) }
|
||||
companion object { fun read(src: Source) = PingPong(src.readInt()) }
|
||||
}
|
||||
|
||||
data class Serial(val serial: String = "") : Packet {
|
||||
companion object { fun read(src: Source) = Serial(src.readString(src.readInt().toLong())) }
|
||||
}
|
||||
|
||||
data class BatteryLevel(val voltage: Float = 0f, val level: Float = 0f) : Packet {
|
||||
companion object {
|
||||
fun read(src: Source): BatteryLevel {
|
||||
val f = src.readSafeFloat()
|
||||
return if (src.remaining >= 4) BatteryLevel(f, src.readSafeFloat()) else BatteryLevel(0f, f)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
data class Tap(override val sensorId: Int = 0, val tap: Int = 0) : SensorSpecificPacket {
|
||||
companion object { fun read(src: Source) = Tap(src.readU8(), src.readU8()) }
|
||||
}
|
||||
|
||||
data class ErrorPacket(override val sensorId: Int = 0, val errorNumber: Int = 0) : SensorSpecificPacket {
|
||||
companion object { fun read(src: Source) = ErrorPacket(src.readU8(), src.readU8()) }
|
||||
}
|
||||
|
||||
data class SensorInfo(
|
||||
override val sensorId: Int = 0,
|
||||
val status: TrackerStatus = TrackerStatus.DISCONNECTED,
|
||||
val imuType: IMUType = IMUType.UNKNOWN,
|
||||
val sensorConfig: UShort? = null,
|
||||
val hasCompletedRestCalibration: Boolean? = null,
|
||||
val trackerPosition: Int? = null,
|
||||
val trackerDataType: Int? = null
|
||||
) : SensorSpecificPacket {
|
||||
companion object {
|
||||
fun read(src: Source) = with(src) {
|
||||
val id = readU8()
|
||||
val stat = TrackerStatus.fromId((readUByte() + 1u).toUByte()) ?: TrackerStatus.DISCONNECTED
|
||||
val imu = if (remaining > 0) IMUType.fromId(readUByte()) ?: IMUType.UNKNOWN else IMUType.UNKNOWN
|
||||
val conf = if (remaining >= 2) readShort().toUShort() else null
|
||||
val calib = if (remaining > 0) readU8() != 0 else null
|
||||
val pos = if (remaining > 0) readU8() else null
|
||||
val dt = if (remaining > 0) readU8() else null
|
||||
SensorInfo(id, stat, imu, conf, calib, pos, dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
data class Rotation2(override val sensorId: Int = 1, val rotation: Quaternion = Quaternion.IDENTITY) : SensorSpecificPacket {
|
||||
companion object { fun read(src: Source) = Rotation2(1, src.readSafeQuat()) }
|
||||
}
|
||||
|
||||
data class RotationData(
|
||||
override val sensorId: Int = 0,
|
||||
val dataType: Int = 0,
|
||||
val rotation: Quaternion = Quaternion.IDENTITY,
|
||||
val calibrationInfo: Int = 0
|
||||
) : SensorSpecificPacket {
|
||||
companion object {
|
||||
fun read(src: Source): RotationData = with(src) {
|
||||
val id = readU8()
|
||||
val type = readU8()
|
||||
val rot = readSafeQuat()
|
||||
val calib = if (remaining > 0) readU8() else 0
|
||||
return RotationData(id, type, rot, calib)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
data class MagnetometerAccuracy(override val sensorId: Int = 0, val accuracy: Float = 0f) : SensorSpecificPacket {
|
||||
companion object { fun read(src: Source) = MagnetometerAccuracy(src.readU8(), src.readSafeFloat()) }
|
||||
}
|
||||
|
||||
data class SignalStrength(override val sensorId: Int = 0, val signal: Int = 0) : SensorSpecificPacket {
|
||||
companion object {
|
||||
fun read(src: Source): SignalStrength {
|
||||
val id = src.readU8()
|
||||
val sig = src.readByte().toInt()
|
||||
return SignalStrength(id, sig)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
data class Temperature(override val sensorId: Int = 0, val temp: Float = 0f) : SensorSpecificPacket {
|
||||
companion object {
|
||||
fun read(src: Source): Temperature {
|
||||
val id = src.readU8()
|
||||
val t = if (src.remaining >= 4) src.readSafeFloat() else 0f
|
||||
return Temperature(id, t)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
data class UserActionPacket(val type: Int = 0) : Packet {
|
||||
companion object { fun read(src: Source) = UserActionPacket(src.readU8()) }
|
||||
}
|
||||
|
||||
data class FeatureFlags(val firmwareFeatures: ByteArray = byteArrayOf()) : Packet {
|
||||
companion object { fun read(src: Source) = FeatureFlags(src.readByteArray(src.remaining.toInt())) }
|
||||
}
|
||||
|
||||
data class RotationAndAccel(
|
||||
override val sensorId: Int = 0,
|
||||
val rotation: Quaternion = Quaternion.IDENTITY,
|
||||
val acceleration: Vector3 = Vector3.NULL
|
||||
) : SensorSpecificPacket {
|
||||
companion object {
|
||||
fun read(src: Source): RotationAndAccel {
|
||||
val id = src.readU8()
|
||||
val scaleR = 1f / 32768f
|
||||
val x = src.readShort() * scaleR
|
||||
val y = src.readShort() * scaleR
|
||||
val z = src.readShort() * scaleR
|
||||
val w = src.readShort() * scaleR
|
||||
val scaleA = 1f / 128f
|
||||
val accel = Vector3(src.readShort() * scaleA, src.readShort() * scaleA, src.readShort() * scaleA)
|
||||
return RotationAndAccel(id, Quaternion(w, x, y, z).unit(), accel)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
data class AckConfigChange(override val sensorId: Int = 0, val configType: UShort = 0u) : SensorSpecificPacket {
|
||||
companion object { fun read(src: Source) = AckConfigChange(src.readU8(), src.readShort().toUShort()) }
|
||||
}
|
||||
|
||||
data class SetConfigFlag(override val sensorId: Int = 255, val configType: UShort = 0u, val state: Boolean = false) : SensorSpecificPacket {
|
||||
override fun write(dst: Sink) {
|
||||
dst.writeUByte(sensorId.toUByte())
|
||||
dst.writeShort(configType.toShort())
|
||||
dst.writeUByte(if (state) 1u else 0u)
|
||||
}
|
||||
}
|
||||
|
||||
data class FlexData(override val sensorId: Int = 0, val flexData: Float = 0f) : SensorSpecificPacket {
|
||||
companion object { fun read(src: Source) = FlexData(src.readU8(), src.readSafeFloat()) }
|
||||
}
|
||||
|
||||
data class PositionPacket(override val sensorId: Int = 0, val position: Vector3 = Vector3.NULL) : SensorSpecificPacket {
|
||||
companion object {
|
||||
fun read(src: Source) = PositionPacket(src.readU8(), Vector3(src.readSafeFloat(), src.readSafeFloat(), src.readSafeFloat()))
|
||||
}
|
||||
}
|
||||
|
||||
data class ProtocolChange(val targetProtocol: Int = 0, val targetVersion: Int = 0) : Packet {
|
||||
override fun write(dst: Sink) {
|
||||
dst.writeUByte(targetProtocol.toUByte())
|
||||
dst.writeUByte(targetVersion.toUByte())
|
||||
}
|
||||
companion object { fun read(src: Source) = ProtocolChange(src.readU8(), src.readU8()) }
|
||||
}
|
||||
|
||||
fun readPacket(type: PacketType, src: Source): Packet = when (type) {
|
||||
PacketType.HEARTBEAT -> Heartbeat
|
||||
PacketType.HANDSHAKE -> Handshake.read(src)
|
||||
PacketType.ROTATION -> Rotation.read(src)
|
||||
PacketType.ACCEL -> Accel.read(src)
|
||||
PacketType.PING_PONG -> PingPong.read(src)
|
||||
PacketType.SERIAL -> Serial.read(src)
|
||||
PacketType.BATTERY_LEVEL -> BatteryLevel.read(src)
|
||||
PacketType.TAP -> Tap.read(src)
|
||||
PacketType.ERROR -> ErrorPacket.read(src)
|
||||
PacketType.SENSOR_INFO -> SensorInfo.read(src)
|
||||
PacketType.ROTATION_2 -> Rotation2.read(src)
|
||||
PacketType.ROTATION_DATA -> RotationData.read(src)
|
||||
PacketType.MAGNETOMETER_ACCURACY -> MagnetometerAccuracy.read(src)
|
||||
PacketType.SIGNAL_STRENGTH -> SignalStrength.read(src)
|
||||
PacketType.TEMPERATURE -> Temperature.read(src)
|
||||
PacketType.USER_ACTION -> UserActionPacket.read(src)
|
||||
PacketType.FEATURE_FLAGS -> FeatureFlags.read(src)
|
||||
PacketType.ROTATION_AND_ACCEL -> RotationAndAccel.read(src)
|
||||
PacketType.ACK_CONFIG_CHANGE -> AckConfigChange.read(src)
|
||||
PacketType.SET_CONFIG_FLAG -> SetConfigFlag() // Usually outbound
|
||||
PacketType.FLEX_DATA -> FlexData.read(src)
|
||||
PacketType.POSITION -> PositionPacket.read(src)
|
||||
PacketType.PROTOCOL_CHANGE -> ProtocolChange.read(src)
|
||||
}
|
||||
|
||||
fun writePacket(dst: Sink, packet: Packet) {
|
||||
val type = when (packet) {
|
||||
is Heartbeat -> PacketType.HEARTBEAT
|
||||
is HandshakeResponse -> PacketType.HANDSHAKE
|
||||
is Rotation -> PacketType.ROTATION
|
||||
is Accel -> PacketType.ACCEL
|
||||
is Handshake -> PacketType.HANDSHAKE
|
||||
is PingPong -> PacketType.PING_PONG
|
||||
is Serial -> PacketType.SERIAL
|
||||
is BatteryLevel -> PacketType.BATTERY_LEVEL
|
||||
is Tap -> PacketType.TAP
|
||||
is Error -> PacketType.ERROR
|
||||
is SensorInfo -> PacketType.SENSOR_INFO
|
||||
is Rotation2 -> PacketType.ROTATION_2
|
||||
is RotationData -> PacketType.ROTATION_DATA
|
||||
is MagnetometerAccuracy -> PacketType.MAGNETOMETER_ACCURACY
|
||||
is SignalStrength -> PacketType.SIGNAL_STRENGTH
|
||||
is Temperature -> PacketType.TEMPERATURE
|
||||
is UserActionPacket -> PacketType.USER_ACTION
|
||||
is SetConfigFlag -> PacketType.SET_CONFIG_FLAG
|
||||
is ProtocolChange -> PacketType.PROTOCOL_CHANGE
|
||||
else -> error("unhandled packet")
|
||||
is FeatureFlags -> PacketType.FEATURE_FLAGS
|
||||
else -> error("Outbound support not implemented for ${packet::class.simpleName}")
|
||||
}
|
||||
|
||||
|
||||
if (type != PacketType.HANDSHAKE) {
|
||||
dst.writeInt(type.id)
|
||||
dst.writeLong(packetNum)
|
||||
dst.writeLong(0)
|
||||
}
|
||||
PacketCodec.write(dst, packet)
|
||||
packet.write(dst)
|
||||
}
|
||||
|
||||
data class PacketEvent<out T : Packet>(
|
||||
@@ -272,9 +323,6 @@ class PacketDispatcher {
|
||||
val listeners = mutableMapOf<KClass<out Packet>, MutableList<(PacketEvent<Packet>) -> Unit>>()
|
||||
private val globalListeners = mutableListOf<(PacketEvent<Packet>) -> Unit>()
|
||||
|
||||
/**
|
||||
* Listen for a specific packet type with metadata.
|
||||
*/
|
||||
@Suppress("UNCHECKED_CAST")
|
||||
inline fun <reified T : Packet> on(crossinline callback: (PacketEvent<T>) -> Unit) {
|
||||
val list = listeners.getOrPut(T::class) { mutableListOf() }
|
||||
|
||||
@@ -19,8 +19,6 @@ data class UDPTrackerServerState(
|
||||
val connections: MutableMap<String, UDPConnection>,
|
||||
)
|
||||
|
||||
const val PACKET_WORKERS = 4
|
||||
|
||||
suspend fun createUDPTrackerServer(
|
||||
serverContext: VRServer,
|
||||
configContext: ConfigContext,
|
||||
@@ -33,46 +31,37 @@ suspend fun createUDPTrackerServer(
|
||||
val selectorManager = SelectorManager(Dispatchers.IO)
|
||||
val serverSocket =
|
||||
aSocket(selectorManager).udp().bind(InetSocketAddress("0.0.0.0", state.port))
|
||||
val packetChannel = Channel<Datagram>(capacity = Channel.BUFFERED)
|
||||
|
||||
supervisorScope {
|
||||
launch(Dispatchers.IO) {
|
||||
while (isActive) {
|
||||
val datagram = serverSocket.receive()
|
||||
packetChannel.send(datagram)
|
||||
}
|
||||
}
|
||||
val packetId = datagram.packet.readInt()
|
||||
val packetNumber = datagram.packet.readLong()
|
||||
val type = PacketType.fromId(packetId) ?: continue
|
||||
val packetData = readPacket(type, datagram.packet)
|
||||
|
||||
repeat(PACKET_WORKERS) { workerId ->
|
||||
launch(Dispatchers.Default) {
|
||||
for (datagram in packetChannel) {
|
||||
val packetId = datagram.packet.readInt()
|
||||
val packetNumber = datagram.packet.readLong()
|
||||
val type = PacketType.fromId(packetId) ?: continue
|
||||
val packetData = PacketCodec.read(type, datagram.packet)
|
||||
val address = datagram.address as InetSocketAddress
|
||||
val connContext = state.connections[address.hostname]
|
||||
|
||||
val address = datagram.address as InetSocketAddress
|
||||
val connContext = state.connections[address.hostname]
|
||||
val event = PacketEvent(
|
||||
data = packetData,
|
||||
packetNumber = packetNumber,
|
||||
)
|
||||
|
||||
val event = PacketEvent(
|
||||
data = packetData,
|
||||
packetNumber = packetNumber,
|
||||
if (connContext !== null) {
|
||||
connContext.packetEvents.emit(event)
|
||||
} else {
|
||||
val newContext = createUDPConnectionContext(
|
||||
id = address.hostname,
|
||||
remoteAddress = address,
|
||||
socket = serverSocket,
|
||||
serverContext = serverContext,
|
||||
scope = this,
|
||||
)
|
||||
|
||||
if (connContext !== null) {
|
||||
connContext.packetEvents.emit(event = event)
|
||||
} else {
|
||||
val newContext = createUDPConnectionContext(
|
||||
id = address.hostname,
|
||||
remoteAddress = address,
|
||||
socket = serverSocket,
|
||||
serverContext = serverContext,
|
||||
scope = this,
|
||||
)
|
||||
|
||||
state.connections[address.hostname] = newContext
|
||||
newContext.packetEvents.emit(event = event)
|
||||
}
|
||||
state.connections[address.hostname] = newContext
|
||||
newContext.packetEvents.emit(event)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,7 +4,7 @@ import dev.slimevr.context.BasicModule
|
||||
import dev.slimevr.context.Context
|
||||
import dev.slimevr.context.createContext
|
||||
import dev.slimevr.tracker.Device
|
||||
import dev.slimevr.tracker.TrackerContext
|
||||
import dev.slimevr.tracker.Tracker
|
||||
import kotlinx.coroutines.CoroutineScope
|
||||
import kotlinx.coroutines.flow.distinctUntilChangedBy
|
||||
import kotlinx.coroutines.flow.launchIn
|
||||
@@ -12,19 +12,19 @@ import kotlinx.coroutines.flow.onEach
|
||||
|
||||
data class VRServerState(
|
||||
val handleId: Int,
|
||||
val trackers: Map<Int, TrackerContext>,
|
||||
val trackers: Map<Int, Tracker>,
|
||||
val devices: Map<Int, Device>,
|
||||
)
|
||||
|
||||
sealed interface VRServerActions {
|
||||
data class NewTracker(val trackerId: Int, val context: TrackerContext) : VRServerActions
|
||||
data class NewTracker(val trackerId: Int, val context: Tracker) : VRServerActions
|
||||
data class NewDevice(val deviceId: Int, val context: Device) : VRServerActions
|
||||
}
|
||||
|
||||
typealias VRServerContext = Context<VRServerState, VRServerActions>
|
||||
typealias VRServerModule = BasicModule<VRServerState, VRServerActions>
|
||||
|
||||
val TestModule = VRServerModule(
|
||||
val BaseModule = VRServerModule(
|
||||
reducer = { s, a ->
|
||||
when (a) {
|
||||
is VRServerActions.NewTracker -> s.copy(
|
||||
@@ -49,8 +49,8 @@ data class VRServer(
|
||||
val context: VRServerContext,
|
||||
) {
|
||||
fun nextHandle() = context.state.value.handleId + 1
|
||||
fun getTrackerContext(id: Int) = context.state.value.trackers[id]
|
||||
fun getDeviceContext(id: Int) = context.state.value.devices[id]
|
||||
fun getTracker(id: Int) = context.state.value.trackers[id]
|
||||
fun getDevice(id: Int) = context.state.value.devices[id]
|
||||
|
||||
companion object {
|
||||
fun create(scope: CoroutineScope): VRServer {
|
||||
@@ -60,7 +60,7 @@ data class VRServer(
|
||||
devices = mapOf(),
|
||||
)
|
||||
|
||||
val modules = listOf(TestModule)
|
||||
val modules = listOf(BaseModule)
|
||||
|
||||
val context = createContext(
|
||||
initialState = server,
|
||||
|
||||
@@ -11,5 +11,7 @@ fun main(args: Array<String>) = runBlocking {
|
||||
val config = createConfig(this)
|
||||
val server = VRServer.create(this)
|
||||
|
||||
val udpServer = createUDPTrackerServer(server, config)
|
||||
createUDPTrackerServer(server, config)
|
||||
|
||||
Unit
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user