diff --git a/server/core/src/main/java/dev/slimevr/context/context.kt b/server/core/src/main/java/dev/slimevr/context/context.kt index 8002b6e9d..73cf82ebd 100644 --- a/server/core/src/main/java/dev/slimevr/context/context.kt +++ b/server/core/src/main/java/dev/slimevr/context/context.kt @@ -39,6 +39,5 @@ fun createContext( } } val context = Context(mutableStateFlow.asStateFlow(), dispatch, dispatchAll, scope) - return context } diff --git a/server/core/src/main/java/dev/slimevr/logger.kt b/server/core/src/main/java/dev/slimevr/logger.kt index c5539dad0..d2055d42b 100644 --- a/server/core/src/main/java/dev/slimevr/logger.kt +++ b/server/core/src/main/java/dev/slimevr/logger.kt @@ -10,6 +10,7 @@ object AppLogger { val tracker = logger("Tracker") val device = logger("Device") val udp = logger("UDPConnection") + val solarxr = logger("SolarXR") init { diff --git a/server/core/src/main/java/dev/slimevr/skeleton/BodyPart.kt b/server/core/src/main/java/dev/slimevr/skeleton/BodyPart.kt deleted file mode 100644 index 92537bee8..000000000 --- a/server/core/src/main/java/dev/slimevr/skeleton/BodyPart.kt +++ /dev/null @@ -1,63 +0,0 @@ -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] - } -} diff --git a/server/core/src/main/java/dev/slimevr/solarxr/server.kt b/server/core/src/main/java/dev/slimevr/solarxr/server.kt index 783f258bb..984cf7742 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/server.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/server.kt @@ -1,5 +1,7 @@ package dev.slimevr.solarxr +import dev.slimevr.AppLogger +import dev.slimevr.VRServer import io.ktor.server.application.* import io.ktor.server.engine.embeddedServer import io.ktor.server.netty.Netty @@ -7,47 +9,54 @@ import io.ktor.server.routing.routing import io.ktor.server.websocket.* import io.ktor.websocket.Frame import io.ktor.websocket.readBytes +import kotlinx.coroutines.runBlocking +import kotlinx.coroutines.sync.Mutex +import kotlinx.coroutines.sync.withLock import solarxr_protocol.MessageBundle import java.nio.ByteBuffer +import kotlin.reflect.KClass -const val SOLARXR_PORT = 21110; - -fun onSolarXRMessage(message: ByteBuffer) { - val messageBundle = MessageBundle.getRootAsMessageBundle(message) +const val SOLARXR_PORT = 21110 - for (index in 0.. { - val data = frame.readBytes() - onSolarXRMessage(frame.buffer) - println("Received Binary Packet: ${data.size} bytes") + is Frame.Binary -> onSolarXRMessage( + frame.buffer, + solarxrConnection + ) + + is Frame.Close -> { + AppLogger.solarxr.info("Connection closed") } + else -> {} } diff --git a/server/core/src/main/java/dev/slimevr/solarxr/solarxr.kt b/server/core/src/main/java/dev/slimevr/solarxr/solarxr.kt index 7f7d097cc..42c1fec33 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/solarxr.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/solarxr.kt @@ -1,6 +1,202 @@ package dev.slimevr.solarxr +import com.google.flatbuffers.FlatBufferBuilder +import dev.slimevr.VRServer +import dev.slimevr.context.Context +import dev.slimevr.context.createContext +import io.ktor.util.moveToByteArray +import kotlinx.coroutines.CoroutineScope +import kotlinx.coroutines.Job +import kotlinx.coroutines.cancelAndJoin +import kotlinx.coroutines.delay +import kotlinx.coroutines.isActive +import kotlinx.coroutines.launch +import kotlinx.coroutines.runBlocking +import kotlinx.coroutines.sync.Mutex +import kotlinx.coroutines.sync.withLock +import solarxr_protocol.MessageBundle +import solarxr_protocol.data_feed.DataFeedConfig +import solarxr_protocol.data_feed.DataFeedMessage +import solarxr_protocol.data_feed.DataFeedMessageHeader +import solarxr_protocol.data_feed.DataFeedUpdate +import solarxr_protocol.data_feed.StartDataFeed +import solarxr_protocol.data_feed.device_data.DeviceData +import solarxr_protocol.data_feed.tracker.TrackerData +import solarxr_protocol.datatypes.DeviceId +import solarxr_protocol.datatypes.TrackerId +import solarxr_protocol.datatypes.hardware_info.HardwareStatus +import solarxr_protocol.rpc.RpcMessage +import kotlin.reflect.KClass +import kotlin.time.measureTime data class SolarXRConnectionState( - val id: Int, + val dataFeedConfigs: List, + val datafeedTimers: List, ) + +sealed interface SolarXRConnectionActions { + data class SetConfig(val configs: List, val timers: List) : + SolarXRConnectionActions +} + +typealias SolarXRConnectionContext = Context + +class PacketDispatcher { + val listeners = mutableMapOf, MutableList Unit>>() + val globalListeners = mutableListOf Unit>() + val mutex = Mutex() + + @Suppress("UNCHECKED_CAST") + inline fun on(crossinline callback: suspend (P) -> Unit) { + runBlocking { + mutex.withLock { + val list = + listeners.getOrPut(P::class as KClass) { mutableListOf() } + list.add { callback(it as P) } + } + } + } + + fun onAny(callback: suspend (T) -> Unit) { + runBlocking { + mutex.withLock { globalListeners.add(callback) } + } + } + + suspend fun emit(event: T) { + val targets = mutex.withLock { + val specific = listeners[event::class]?.toList() ?: emptyList() + val global = globalListeners.toList() + global + specific + } + targets.forEach { it(event) } + } +} + +data class SolarXRConnection( + val context: SolarXRConnectionContext, + val serverContext: VRServer, + val dataFeedDispatcher: PacketDispatcher, + val rpcDispatcher: PacketDispatcher, + val send: suspend (ByteArray) -> Unit +) + +data class SolarXRConnectionModule( + val reducer: ((SolarXRConnectionState, SolarXRConnectionActions) -> SolarXRConnectionState)? = null, + val observer: ((SolarXRConnection) -> Unit)? = null, +) + + +val DataFeedInitModule = SolarXRConnectionModule( + observer = { context -> + context.dataFeedDispatcher.on { event -> + val datafeeds = event.dataFeeds ?: return@on + val state = context.context.state.value + + state.datafeedTimers.forEach { + it.cancelAndJoin() + } + + val timers = datafeeds.map { config -> + val minTime = config.minimumTimeSinceLast ?: return@map null + + return@map context.context.scope.launch { + val fbb = FlatBufferBuilder(1024) + while (isActive) { + val timeTaken = measureTime { + fbb.clear() + + val serverState = context.serverContext.context.state.value + val trackers = + serverState.trackers.values.map { it.context.state.value } + val devices = + serverState.devices.values.map { it.context.state.value } + .map { device -> + DeviceData( + id = DeviceId(device.id.toUByte()), + hardwareStatus = HardwareStatus( + batteryVoltage = device.batteryVoltage, + batteryPctEstimate = device.batteryLevel.toUInt() + .toUByte(), + ping = device.ping?.toUShort() + ), + trackers = trackers.filter { it.deviceId == device.id } + .map { tracker -> + TrackerData( + trackerId = TrackerId( + trackerNum = tracker.id.toUByte(), + deviceId = DeviceId(device.id.toUByte()) + ), + status = tracker.status + ) + } + ) + } + + + fbb.finish( + MessageBundle( + dataFeedMsgs = listOf( + DataFeedMessageHeader( + message = DataFeedUpdate( + devices = devices + ) + ) + ) + ).encode(fbb) + ) + + context.send(fbb.dataBuffer().moveToByteArray()) + } + val remainingDelay = + (minTime.toLong() - timeTaken.inWholeMilliseconds).coerceAtLeast( + 0 + ) + delay(remainingDelay) + } + } + }.filterNotNull() + + context.context.dispatch( + SolarXRConnectionActions.SetConfig( + datafeeds, + timers = timers + ) + ) + + timers.forEach { it.start() } + } + } +) + +fun createSolarXRConnection( + serverContext: VRServer, + onSend: suspend (ByteArray) -> Unit, + scope: CoroutineScope +): SolarXRConnection { + + val state = SolarXRConnectionState( + dataFeedConfigs = listOf(), + datafeedTimers = listOf() + ) + + val modules = listOf(DataFeedInitModule) + + val context = createContext( + initialState = state, + reducers = modules.map { it.reducer }, + scope = scope, + ) + + val conn = SolarXRConnection( + context = context, + serverContext = serverContext, + dataFeedDispatcher = PacketDispatcher(), + rpcDispatcher = PacketDispatcher(), + onSend, + ) + + modules.map { it.observer }.forEach { it?.invoke(conn) } + + return conn +} diff --git a/server/core/src/main/java/dev/slimevr/tracker/imu.kt b/server/core/src/main/java/dev/slimevr/tracker/imu.kt deleted file mode 100644 index 7fcd9e57a..000000000 --- a/server/core/src/main/java/dev/slimevr/tracker/imu.kt +++ /dev/null @@ -1,30 +0,0 @@ -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] - } -} diff --git a/server/core/src/main/java/dev/slimevr/tracker/tracker.kt b/server/core/src/main/java/dev/slimevr/tracker/tracker.kt index 7a5393305..b4edeb695 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/tracker.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/tracker.kt @@ -1,29 +1,16 @@ package dev.slimevr.tracker -import dev.slimevr.AppLogger 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.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] - } -} +import solarxr_protocol.datatypes.BodyPart +import solarxr_protocol.datatypes.TrackerStatus +import solarxr_protocol.datatypes.hardware_info.ImuType data class TrackerIdNum(val id: Int, val trackerNum: Int) @@ -31,7 +18,7 @@ data class TrackerState( val id: Int, val name: String, val hardwareId: String, - val sensorType: IMUType, + val sensorType: ImuType, val bodyPart: BodyPart?, val status: TrackerStatus, val customName: String?, @@ -65,7 +52,7 @@ fun createTracker( scope: CoroutineScope, id: Int, deviceId: Int, - sensorType: IMUType, + sensorType: ImuType, hardwareId: String, origin: DeviceOrigin, serverContext: VRServer diff --git a/server/core/src/main/java/dev/slimevr/tracker/udp/connection.kt b/server/core/src/main/java/dev/slimevr/tracker/udp/connection.kt index 8877f3304..1bc589623 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/udp/connection.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/udp/connection.kt @@ -168,7 +168,6 @@ val HandshakeModule = UDPConnectionModule( didHandshake = true, deviceId = a.deviceId ) - else -> s } }, diff --git a/server/core/src/main/java/dev/slimevr/tracker/udp/packets.kt b/server/core/src/main/java/dev/slimevr/tracker/udp/packets.kt index 262397311..a3eb7d8ae 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/udp/packets.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/udp/packets.kt @@ -1,7 +1,5 @@ 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 @@ -15,6 +13,8 @@ import kotlinx.io.readFloat import kotlinx.io.readString import kotlinx.io.readUByte import kotlinx.io.writeUByte +import solarxr_protocol.datatypes.TrackerStatus +import solarxr_protocol.datatypes.hardware_info.ImuType import kotlin.reflect.KClass private fun Source.readU8(): Int = readByte().toInt() and 0xFF @@ -148,7 +148,7 @@ data class ErrorPacket(override val sensorId: Int = 0, val errorNumber: Int = 0) data class SensorInfo( override val sensorId: Int = 0, val status: TrackerStatus = TrackerStatus.DISCONNECTED, - val imuType: IMUType = IMUType.UNKNOWN, + val imuType: ImuType = ImuType.Other, val sensorConfig: UShort? = null, val hasCompletedRestCalibration: Boolean? = null, val trackerPosition: Int? = null, @@ -157,8 +157,8 @@ data class SensorInfo( 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 stat = TrackerStatus.fromValue((readUByte() + 1u).toUByte()) ?: TrackerStatus.DISCONNECTED + val imu = if (remaining > 0) ImuType.fromValue(readUByte().toUShort()) ?: ImuType.Other else ImuType.Other 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 diff --git a/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt b/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt index b5546429f..ba14c6999 100644 --- a/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt +++ b/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt @@ -17,7 +17,7 @@ fun main(args: Array) = runBlocking { createUDPTrackerServer(server, config) } launch { - createSolarXRWebsocketServer() + createSolarXRWebsocketServer(server) } Unit }