diff --git a/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt b/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt index a78d06bcc..9dfcd3d0b 100644 --- a/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt +++ b/server/android/src/main/java/dev/slimevr/android/ForegroundService.kt @@ -24,6 +24,7 @@ import dev.slimevr.android.hid.createAndroidHIDManager import dev.slimevr.android.ipc.createAndroidSolarXRWebsocketServer import dev.slimevr.android.serial.AndroidFirmwareFlasher import dev.slimevr.android.serial.createAndroidSerialServer +import dev.slimevr.android.timing.createAndroidWaiter import dev.slimevr.android.udp.resolveAndroidUdpAddress import dev.slimevr.android.vrchat.resolveAndroidLocalIpAddress import dev.slimevr.bvh.BVHManager @@ -154,7 +155,7 @@ class ForegroundService : Service() { val firmwareManager = FirmwareManager.create(ctx = phase1, scope = scope, flasher = AndroidFirmwareFlasher) val networkProfileManager = NetworkProfileManager.create(scope = scope, isSupported = false) - val skeleton = Skeleton.create(scope = scope, ctx = phase1) + val skeleton = Skeleton.create(scope = scope, ctx = phase1, waiter = createAndroidWaiter()) val provisioningManager = ProvisioningManager.create(ctx = phase1, scope = scope) val heightCalibrationManager = HeightCalibrationManager.create(ctx = phase1, scope = scope) val trackingChecklist = TrackingChecklist.create(scope = scope) diff --git a/server/android/src/main/java/dev/slimevr/android/timing/waiter.kt b/server/android/src/main/java/dev/slimevr/android/timing/waiter.kt new file mode 100644 index 000000000..1111b7577 --- /dev/null +++ b/server/android/src/main/java/dev/slimevr/android/timing/waiter.kt @@ -0,0 +1,11 @@ +package dev.slimevr.android.timing + +import dev.slimevr.util.PreciseWaiter +import java.util.concurrent.locks.LockSupport +import kotlin.time.Duration + +fun createAndroidWaiter(): PreciseWaiter = ParkWaiter + +private object ParkWaiter : PreciseWaiter { + override fun sleep(duration: Duration) = LockSupport.parkNanos(duration.inWholeNanoseconds) +} diff --git a/server/core/src/main/java/dev/slimevr/skeleton/module.kt b/server/core/src/main/java/dev/slimevr/skeleton/module.kt index fab22eee8..624f5b340 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -17,6 +17,7 @@ import dev.slimevr.skeleton.inputprocessors.HeadPositionFallbackProcessor import dev.slimevr.skeleton.inputprocessors.HipYawRollAlignInputProcessor import dev.slimevr.skeleton.inputprocessors.SpineImputeInputProcessor import dev.slimevr.skeleton.inputprocessors.UpperLegsRollAlignInputProcessor +import dev.slimevr.util.PreciseWaiter import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 import kotlinx.coroutines.CoroutineScope @@ -180,7 +181,7 @@ class Skeleton( companion object { const val DEFAULT_HZ = 500 - fun create(scope: CoroutineScope, ctx: Phase1ContextProvider, hz: Int = DEFAULT_HZ): Skeleton { + fun create(scope: CoroutineScope, ctx: Phase1ContextProvider, waiter: PreciseWaiter, hz: Int = DEFAULT_HZ): Skeleton { val settings = ctx.config.settings val behaviours = listOf( ProportionsBehaviour(ctx.config.userConfig), @@ -189,6 +190,7 @@ class Skeleton( // YouSpinMeRightRoundBehaviour(inputHz = 50f), ComputedSkeletonBehaviour( hz = hz, + waiter = waiter, inputProcessors = listOf( BonePredictionInputProcessor(settings), BoneSmoothingInputProcessor(settings), diff --git a/server/core/src/main/java/dev/slimevr/solarxr/datafeed/datafeed.kt b/server/core/src/main/java/dev/slimevr/solarxr/datafeed/datafeed.kt index b09c30cda..da85f17d3 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/datafeed/datafeed.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/datafeed/datafeed.kt @@ -15,8 +15,6 @@ import dev.slimevr.solarxr.createBone import dev.slimevr.tracker.Motion import dev.slimevr.tracker.TrackerState import kotlinx.coroutines.cancelAndJoin -import kotlinx.coroutines.delay -import kotlinx.coroutines.isActive import kotlinx.coroutines.launch import solarxr_protocol.data_feed.DataFeedConfig import solarxr_protocol.data_feed.DataFeedMessageHeader @@ -40,6 +38,8 @@ import solarxr_protocol.datatypes.hardware_info.HardwareStatus import solarxr_protocol.datatypes.hardware_info.ImuType import solarxr_protocol.datatypes.math.Quat import solarxr_protocol.datatypes.math.Vec3f +import kotlin.time.Duration.Companion.milliseconds +import kotlin.time.TimeSource private fun ipv4AddressFromString(address: String): UInt { val parts = address.split('.') @@ -193,7 +193,11 @@ fun createDatafeedFrame( ) } -class DataFeedInitBehaviour(val server: VRServer, val skeleton: Skeleton) : SolarXRBridgeBehaviour { +class DataFeedInitBehaviour( + val server: VRServer, + val skeleton: Skeleton, + private val timeSource: TimeSource.WithComparableMarks, +) : SolarXRBridgeBehaviour { override fun observe(receiver: SolarXRBridge) { receiver.dataFeedDispatcher.on { event -> val dataFeeds = event.dataFeeds ?: return@on @@ -202,8 +206,13 @@ class DataFeedInitBehaviour(val server: VRServer, val skeleton: Skeleton) : Sola val timers = dataFeeds.mapIndexed { index, config -> receiver.context.scope.launch { - val minTime = config.minimumTimeSinceLast.toLong() - while (isActive) { + val interval = config.minimumTimeSinceLast.toLong().milliseconds + var nextSend = timeSource.markNow() + + skeleton.computed.collect { + val now = timeSource.markNow() + if (now < nextSend) return@collect + try { receiver.sendDataFeed( createDatafeedFrame( @@ -218,7 +227,9 @@ class DataFeedInitBehaviour(val server: VRServer, val skeleton: Skeleton) : Sola } catch (e: Exception) { AppLogger.solarxr.error(e, "Error sending data feed") } - delay(minTime) + + nextSend += interval + if (nextSend < now) nextSend = now + interval } } } diff --git a/server/core/src/main/java/dev/slimevr/solarxr/module.kt b/server/core/src/main/java/dev/slimevr/solarxr/module.kt index fa06f8632..8b068d90b 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/module.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/module.kt @@ -39,6 +39,7 @@ import dev.slimevr.solarxr.rpc.VmcBehaviour import dev.slimevr.solarxr.rpc.VrcBehaviour import dev.slimevr.solarxr.rpc.VrcOscBehaviour import dev.slimevr.tracker.TrackerActions +import dev.slimevr.util.timeSource import kotlinx.coroutines.CoroutineScope import kotlinx.coroutines.Job import solarxr_protocol.MessageBundle @@ -169,7 +170,7 @@ class SolarXRBridge( companion object { fun buildBehaviours(appContext: AppContextProvider): List = buildList { - add(DataFeedInitBehaviour(appContext.server, appContext.skeleton)) + add(DataFeedInitBehaviour(appContext.server, appContext.skeleton, timeSource)) add(SerialBehaviour(appContext.serialServer)) add(FirmwareBehaviour(appContext.server, appContext.firmwareManager)) appContext.vrcConfigManager?.let { vrc -> diff --git a/server/core/src/main/java/dev/slimevr/util/precise-waiter.kt b/server/core/src/main/java/dev/slimevr/util/precise-waiter.kt new file mode 100644 index 000000000..e63d9f777 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/util/precise-waiter.kt @@ -0,0 +1,10 @@ +package dev.slimevr.util + +import kotlin.time.Duration + +/** + * Blocks the calling thread for a requested duration, as precisely as the platform allows. + */ +fun interface PreciseWaiter { + fun sleep(duration: Duration) +} diff --git a/server/core/src/test/java/dev/slimevr/solarxr/DataFeedTest.kt b/server/core/src/test/java/dev/slimevr/solarxr/DataFeedTest.kt index 8f2bd34d4..c8cd60e7a 100644 --- a/server/core/src/test/java/dev/slimevr/solarxr/DataFeedTest.kt +++ b/server/core/src/test/java/dev/slimevr/solarxr/DataFeedTest.kt @@ -9,6 +9,7 @@ import dev.slimevr.buildTestSkeleton import dev.slimevr.buildTestUserConfig import dev.slimevr.buildTestVrServer import dev.slimevr.context.Context +import dev.slimevr.skeleton.Skeleton import dev.slimevr.solarxr.datafeed.DataFeedInitBehaviour import kotlinx.coroutines.ExperimentalCoroutinesApi import kotlinx.coroutines.test.TestScope @@ -21,9 +22,13 @@ import solarxr_protocol.data_feed.PollDataFeed import solarxr_protocol.data_feed.StartDataFeed import kotlin.test.Test import kotlin.test.assertEquals +import kotlin.time.Duration +import kotlin.time.Duration.Companion.milliseconds + +private class TestConn(val bridge: SolarXRBridge, val skeleton: Skeleton) @OptIn(ExperimentalCoroutinesApi::class) -private fun TestScope.testConn(onSend: suspend (ByteArray) -> Unit): SolarXRBridge { +private fun TestScope.testConn(onSend: suspend (ByteArray) -> Unit): TestConn { val server = buildTestVrServer(backgroundScope) val skeleton = buildTestSkeleton(backgroundScope) val settings = buildTestSettings(backgroundScope) @@ -40,7 +45,7 @@ private fun TestScope.testConn(onSend: suspend (ByteArray) -> Unit): SolarXRBrid initialState = SolarXRBridgeState(dataFeedConfigs = listOf()), scope = backgroundScope, reducer = ::reduce, - behaviours = listOf(DataFeedInitBehaviour(server, skeleton)), + behaviours = listOf(DataFeedInitBehaviour(server, skeleton, testScheduler.timeSource)), name = "SolarXRDataFeedTest", ) val bridge = SolarXRBridge( @@ -56,11 +61,26 @@ private fun TestScope.testConn(onSend: suspend (ByteArray) -> Unit): SolarXRBrid // launchIn registers the handler synchronously, but the dispatcher's own drain loop still has to // start before anything emitted here can reach it. runCurrent() - return bridge + return TestConn(bridge, skeleton) } private fun config(intervalMs: Int) = DataFeedConfig(minimumTimeSinceLast = intervalMs.toUShort()) +/** Drives [Skeleton.computed] the way the 500Hz loop does, so the feed's gate has a clock to ride */ +@OptIn(ExperimentalCoroutinesApi::class) +private fun TestScope.tickSkeleton(conn: TestConn, duration: Duration, tick: Duration = 2.milliseconds) { + // Let a freshly started feed subscribe and take the replayed frame before the clock moves + runCurrent() + val frame = conn.skeleton.currentComputed + var elapsed = Duration.ZERO + while (elapsed < duration) { + advanceTimeBy(tick) + conn.skeleton.computed.tryEmit(frame) + runCurrent() + elapsed += tick + } +} + @OptIn(ExperimentalCoroutinesApi::class) class DataFeedTest { @@ -69,10 +89,10 @@ class DataFeedTest { var sendCount = 0 val conn = testConn { sendCount++ } - conn.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) + conn.bridge.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) // fires at t=0, t=100, t=200 - advanceTimeBy(250) + tickSkeleton(conn, 250.milliseconds) assertEquals(3, sendCount) } @@ -81,11 +101,11 @@ class DataFeedTest { var sendCount = 0 val conn = testConn { sendCount++ } - conn.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100), config(200)))) + conn.bridge.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100), config(200)))) // 100ms feed: t=0, t=100, t=200 -> 3 sends // 200ms feed: t=0, t=200 -> 2 sends - advanceTimeBy(250) + tickSkeleton(conn, 250.milliseconds) assertEquals(5, sendCount) } @@ -94,9 +114,9 @@ class DataFeedTest { var sendCount = 0 val conn = testConn { sendCount++ } - conn.dataFeedDispatcher.emit(PollDataFeed(config = config(100))) + conn.bridge.dataFeedDispatcher.emit(PollDataFeed(config = config(100))) - advanceTimeBy(500) + tickSkeleton(conn, 500.milliseconds) assertEquals(1, sendCount) } @@ -105,14 +125,14 @@ class DataFeedTest { var sendCount = 0 val conn = testConn { sendCount++ } - conn.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) - advanceTimeBy(250) + conn.bridge.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) + tickSkeleton(conn, 250.milliseconds) assertEquals(3, sendCount) - conn.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) + conn.bridge.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) sendCount = 0 - advanceTimeBy(250) + tickSkeleton(conn, 250.milliseconds) assertEquals(3, sendCount) } @@ -121,14 +141,14 @@ class DataFeedTest { var sendCount = 0 val conn = testConn { sendCount++ } - conn.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) - advanceTimeBy(250) + conn.bridge.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = listOf(config(100)))) + tickSkeleton(conn, 250.milliseconds) assertEquals(3, sendCount) - conn.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = emptyList())) + conn.bridge.dataFeedDispatcher.emit(StartDataFeed(dataFeeds = emptyList())) sendCount = 0 - advanceTimeBy(500) + tickSkeleton(conn, 500.milliseconds) assertEquals(0, sendCount) } 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 9b4202064..1f87fb694 100644 --- a/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt +++ b/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt @@ -24,6 +24,7 @@ import dev.slimevr.desktop.keybind.isGnome import dev.slimevr.desktop.networkprofile.setupDesktopNetworkProfileChecker import dev.slimevr.desktop.serial.DesktopFirmwareFlasher import dev.slimevr.desktop.serial.createDesktopSerialServer +import dev.slimevr.desktop.timing.createDesktopWaiter import dev.slimevr.desktop.trackingchecklist.SteamVRCheckBehaviour import dev.slimevr.desktop.udp.resolveDesktopUdpAddress import dev.slimevr.desktop.vrchat.createDesktopVRCConfigManager @@ -126,7 +127,7 @@ fun main(args: Array) = runBlocking(appCoroutineExceptionHandler + val firmwareManager = FirmwareManager.create(ctx = phase1, scope = this, flasher = DesktopFirmwareFlasher) val vrcConfigManager = createDesktopVRCConfigManager(ctx = phase1, scope = this) val networkProfileManager = NetworkProfileManager.create(scope = this, isSupported = CURRENT_PLATFORM == Platform.WINDOWS) - val skeleton = Skeleton.create(scope = this, ctx = phase1) + val skeleton = Skeleton.create(scope = this, ctx = phase1, waiter = createDesktopWaiter()) val provisioningManager = ProvisioningManager.create(ctx = phase1, scope = this) val heightCalibrationManager = HeightCalibrationManager.create(ctx = phase1, scope = this) val trackingChecklist = TrackingChecklist.create(scope = this, extraBehaviours = { appContext -> diff --git a/server/desktop/src/main/java/dev/slimevr/desktop/timing/waiter.kt b/server/desktop/src/main/java/dev/slimevr/desktop/timing/waiter.kt new file mode 100644 index 000000000..db2ee927d --- /dev/null +++ b/server/desktop/src/main/java/dev/slimevr/desktop/timing/waiter.kt @@ -0,0 +1,97 @@ +package dev.slimevr.desktop.timing + +import dev.slimevr.CURRENT_PLATFORM +import dev.slimevr.Platform +import dev.slimevr.logging.AppLogger +import dev.slimevr.util.PreciseWaiter +import java.lang.foreign.Arena +import java.lang.foreign.FunctionDescriptor +import java.lang.foreign.Linker +import java.lang.foreign.MemorySegment +import java.lang.foreign.SymbolLookup +import java.lang.foreign.ValueLayout.ADDRESS +import java.lang.foreign.ValueLayout.JAVA_INT +import java.lang.foreign.ValueLayout.JAVA_LONG +import java.util.concurrent.locks.LockSupport +import kotlin.time.Duration + +/** The [PreciseWaiter] this platform can hold a timed loop to */ +suspend fun createDesktopWaiter(): PreciseWaiter = when (CURRENT_PLATFORM) { + // A Windows park resolves to the scheduler tick, 1ms at best and 15.6ms by default, so a 2ms + // deadline wakes two ticks late and a 500Hz loop settles near 330Hz + Platform.WINDOWS -> createWindowsWaiter() + Platform.LINUX, Platform.OSX, Platform.UNKNOWN -> ParkWaiter +} + +private suspend fun createWindowsWaiter(): PreciseWaiter = runCatching { WindowsWaitableTimer.create() } + .onSuccess { AppLogger.skeleton.info("Timed loops waiting on the Windows high-resolution waitable timer") } + .getOrElse { + AppLogger.skeleton.warn(it, "No high-resolution timer, timed loops fall back to park granularity") + ParkWaiter + } + +/** The JVM park, for platforms whose timer needs no help and for when the binding won't load */ +private object ParkWaiter : PreciseWaiter { + override fun sleep(duration: Duration) = LockSupport.parkNanos(duration.inWholeNanoseconds) +} + +private const val CREATE_WAITABLE_TIMER_MANUAL_RESET = 0x1 +private const val CREATE_WAITABLE_TIMER_HIGH_RESOLUTION = 0x2 +private const val TIMER_ALL_ACCESS = 0x1F0003 +private const val INFINITE = -1 +private const val NANOS_PER_TICK = 100 + +/** + * One waitable timer, bound to the thread that created it. + */ +private class WindowsWaitableTimer private constructor( + private val handle: MemorySegment, + private val dueTime: MemorySegment, +) : PreciseWaiter { + override fun sleep(duration: Duration) { + dueTime.set(JAVA_LONG, 0, -(duration.inWholeNanoseconds / NANOS_PER_TICK)) + val armed = setWaitableTimer.invokeExact( + handle, + dueTime, + 0, + MemorySegment.NULL, + MemorySegment.NULL, + 0, + ) as Int + if (armed == 0) return + waitForSingleObject.invokeExact(handle, INFINITE) as Int + } + + companion object { + private val linker = Linker.nativeLinker() + private val kernel32 = SymbolLookup.libraryLookup("kernel32.dll", Arena.global()) + + private val createWaitableTimerEx = linker.downcallHandle( + kernel32.findOrThrow("CreateWaitableTimerExW"), + FunctionDescriptor.of(ADDRESS, ADDRESS, ADDRESS, JAVA_INT, JAVA_INT), + ) + + private val setWaitableTimer = linker.downcallHandle( + kernel32.findOrThrow("SetWaitableTimer"), + FunctionDescriptor.of(JAVA_INT, ADDRESS, ADDRESS, JAVA_INT, ADDRESS, ADDRESS, JAVA_INT), + ) + + private val waitForSingleObject = linker.downcallHandle( + kernel32.findOrThrow("WaitForSingleObject"), + FunctionDescriptor.of(JAVA_INT, ADDRESS, JAVA_INT), + ) + + /** Throws when the binding won't load or the OS predates the high-resolution flag */ + fun create(): WindowsWaitableTimer { + val handle = createWaitableTimerEx.invokeExact( + MemorySegment.NULL, + MemorySegment.NULL, + CREATE_WAITABLE_TIMER_MANUAL_RESET or CREATE_WAITABLE_TIMER_HIGH_RESOLUTION, + TIMER_ALL_ACCESS, + ) as MemorySegment + check(handle.address() != 0L) { "CreateWaitableTimerExW returned NULL" } + + return WindowsWaitableTimer(handle, Arena.ofAuto().allocate(JAVA_LONG)) + } + } +} diff --git a/solarxr-protocol b/solarxr-protocol index 564e26185..f1bf9d386 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit 564e2618571f3c978201c821183934cab53132e9 +Subproject commit f1bf9d386f56ec1c371b44d7de862034a4844fa8