diff --git a/server/core/src/main/java/dev/slimevr/heightcalibration/behaviours.kt b/server/core/src/main/java/dev/slimevr/heightcalibration/behaviours.kt new file mode 100644 index 000000000..1134d5410 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/heightcalibration/behaviours.kt @@ -0,0 +1,186 @@ +@file:OptIn(kotlinx.coroutines.FlowPreview::class) + +package dev.slimevr.heightcalibration + +import io.github.axisangles.ktmath.Vector3 +import kotlinx.coroutines.flow.sample +import kotlinx.coroutines.flow.takeWhile +import kotlinx.coroutines.withTimeoutOrNull +import solarxr_protocol.rpc.UserHeightCalibrationStatus +import kotlin.math.PI +import kotlin.math.cos +import kotlin.math.sqrt + +internal const val SAMPLE_INTERVAL_MS = 16L + +private const val FLOOR_ALPHA = 0.1f +private const val HMD_ALPHA = 0.1f + +private const val CONTROLLER_STABILITY_THRESHOLD = 0.005f +internal const val CONTROLLER_STABILITY_DURATION = 300_000_000L + +private const val HMD_STABILITY_THRESHOLD = 0.003f +internal const val HEAD_STABILITY_DURATION = 600_000_000L + +internal const val MAX_FLOOR_Y = 0.10f +internal const val HMD_RISE_THRESHOLD = 1.2f +internal const val HEIGHT_MIN = 1.4f +internal const val HEIGHT_MAX = 1.936f + +private val HEAD_ANGLE_THRESHOLD = cos((PI / 180.0) * 15.0) +private val CONTROLLER_ANGLE_THRESHOLD = cos((PI / 180.0) * 45.0) + +internal const val TIMEOUT_MS = 30_000L + +private fun UserHeightCalibrationStatus.isTerminal() = when (this) { + UserHeightCalibrationStatus.DONE, + UserHeightCalibrationStatus.ERROR_TOO_HIGH, + UserHeightCalibrationStatus.ERROR_TOO_SMALL, + -> true + else -> false +} + +private fun isControllerPointingDown(snapshot: TrackerSnapshot): Boolean { + val forward = snapshot.rotation.sandwich(Vector3.NEG_Z) + return (forward dot Vector3.NEG_Y) >= CONTROLLER_ANGLE_THRESHOLD +} + +private fun isHmdLeveled(snapshot: TrackerSnapshot): Boolean { + val up = snapshot.rotation.sandwich(Vector3.POS_Y) + return (up dot Vector3.POS_Y) >= HEAD_ANGLE_THRESHOLD +} + +object CalibrationBehaviour : HeightCalibrationBehaviourType { + override fun reduce(state: HeightCalibrationState, action: HeightCalibrationActions) = + when (action) { + is HeightCalibrationActions.Update -> state.copy( + status = action.status, + currentHeight = action.currentHeight, + ) + } +} + +internal suspend fun runCalibrationSession( + context: HeightCalibrationContext, + hmdUpdates: kotlinx.coroutines.flow.Flow, + controllerUpdates: kotlinx.coroutines.flow.Flow, + clock: () -> Long = System::nanoTime, +) { + var currentFloorLevel = Float.MAX_VALUE + var currentHeight = 0f + var floorStableStart: Long? = null + var heightStableStart: Long? = null + + var floorFiltered: Vector3? = null + var floorEnergyEma = 0f + var hmdFiltered: Vector3? = null + var hmdEnergyEma = 0f + + fun dispatch(status: UserHeightCalibrationStatus, height: Float = currentHeight) { + currentHeight = height + context.dispatch(HeightCalibrationActions.Update(status, height)) + } + + dispatch(UserHeightCalibrationStatus.RECORDING_FLOOR) + + withTimeoutOrNull(TIMEOUT_MS) { + + // Floor phase: collect controller updates until the floor level is locked in + controllerUpdates + .sample(SAMPLE_INTERVAL_MS) + .takeWhile { context.state.value.status != UserHeightCalibrationStatus.WAITING_FOR_RISE } + .collect { snapshot -> + val now = clock() + + if (snapshot.position.y > MAX_FLOOR_Y) { + floorStableStart = null + floorFiltered = null + floorEnergyEma = 0f + return@collect + } + + if (!isControllerPointingDown(snapshot)) { + dispatch(UserHeightCalibrationStatus.WAITING_FOR_CONTROLLER_PITCH) + floorStableStart = null + floorFiltered = null + floorEnergyEma = 0f + return@collect + } + + val pos = snapshot.position + val prev = floorFiltered ?: pos + val newFiltered = prev * (1f - FLOOR_ALPHA) + pos * FLOOR_ALPHA + floorFiltered = newFiltered + currentFloorLevel = minOf(currentFloorLevel, pos.y) + + val dev = pos - newFiltered + floorEnergyEma = floorEnergyEma * (1f - FLOOR_ALPHA) + (dev dot dev) * FLOOR_ALPHA + + if (sqrt(floorEnergyEma) > CONTROLLER_STABILITY_THRESHOLD) { + floorStableStart = null + floorFiltered = null + floorEnergyEma = 0f + return@collect + } + + val stableStart = floorStableStart ?: now.also { floorStableStart = it } + if (now - stableStart >= CONTROLLER_STABILITY_DURATION) { + dispatch(UserHeightCalibrationStatus.WAITING_FOR_RISE) + } + } + + // Height phase: collect HMD updates until a terminal status is reached + hmdUpdates + .sample(SAMPLE_INTERVAL_MS) + .takeWhile { !context.state.value.status.isTerminal() } + .collect { snapshot -> + val now = clock() + val relativeY = snapshot.position.y - currentFloorLevel + + if (relativeY <= HMD_RISE_THRESHOLD) { + dispatch(UserHeightCalibrationStatus.WAITING_FOR_RISE, relativeY) + heightStableStart = null + hmdFiltered = null + hmdEnergyEma = 0f + return@collect + } + + if (!isHmdLeveled(snapshot)) { + dispatch(UserHeightCalibrationStatus.WAITING_FOR_FW_LOOK, relativeY) + heightStableStart = null + hmdFiltered = null + hmdEnergyEma = 0f + return@collect + } + + dispatch(UserHeightCalibrationStatus.RECORDING_HEIGHT, relativeY) + + val pos = snapshot.position + val prev = hmdFiltered ?: pos + val newFiltered = prev * (1f - HMD_ALPHA) + pos * HMD_ALPHA + hmdFiltered = newFiltered + + val dev = pos - newFiltered + hmdEnergyEma = hmdEnergyEma * (1f - HMD_ALPHA) + (dev dot dev) * HMD_ALPHA + + if (sqrt(hmdEnergyEma) > HMD_STABILITY_THRESHOLD) { + heightStableStart = null + hmdFiltered = null + hmdEnergyEma = 0f + return@collect + } + + val stableStart = heightStableStart ?: now.also { heightStableStart = it } + if (now - stableStart >= HEAD_STABILITY_DURATION) { + val finalStatus = when { + relativeY < HEIGHT_MIN -> UserHeightCalibrationStatus.ERROR_TOO_SMALL + relativeY > HEIGHT_MAX -> UserHeightCalibrationStatus.ERROR_TOO_HIGH + else -> UserHeightCalibrationStatus.DONE + } + dispatch(finalStatus, relativeY) + + // TODO (when DONE): persist height to config, update user proportions, clear mounting reset flags + } + } + } ?: dispatch(UserHeightCalibrationStatus.ERROR_TIMEOUT) +} diff --git a/server/core/src/main/java/dev/slimevr/heightcalibration/module.kt b/server/core/src/main/java/dev/slimevr/heightcalibration/module.kt new file mode 100644 index 000000000..9835e1f93 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/heightcalibration/module.kt @@ -0,0 +1,102 @@ +@file:OptIn(kotlinx.coroutines.ExperimentalCoroutinesApi::class) + +package dev.slimevr.heightcalibration + +import dev.slimevr.VRServer +import dev.slimevr.context.Behaviour +import dev.slimevr.context.Context +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import kotlinx.coroutines.CoroutineScope +import kotlinx.coroutines.Job +import kotlinx.coroutines.flow.Flow +import kotlinx.coroutines.flow.combine +import kotlinx.coroutines.flow.emptyFlow +import kotlinx.coroutines.flow.flatMapLatest +import kotlinx.coroutines.flow.map +import kotlinx.coroutines.launch +import solarxr_protocol.datatypes.BodyPart +import solarxr_protocol.rpc.UserHeightCalibrationStatus + +data class TrackerSnapshot(val position: Vector3, val rotation: Quaternion) + +data class HeightCalibrationState( + val status: UserHeightCalibrationStatus, + val currentHeight: Float, +) + +sealed interface HeightCalibrationActions { + data class Update(val status: UserHeightCalibrationStatus, val currentHeight: Float) : HeightCalibrationActions +} + +typealias HeightCalibrationContext = Context +typealias HeightCalibrationBehaviourType = Behaviour + +val INITIAL_HEIGHT_CALIBRATION_STATE = HeightCalibrationState( + status = UserHeightCalibrationStatus.NONE, + currentHeight = 0f, +) + +class HeightCalibrationManager( + val context: HeightCalibrationContext, + val serverContext: VRServer, +) { + private var sessionJob: Job? = null + + // These Flows do nothing until the calibration use collect on it + val hmdUpdates: Flow = serverContext.context.state + .flatMapLatest { state -> + val hmd = state.trackers.values + .find { it.context.state.value.bodyPart == BodyPart.HEAD } // TODO: Need to check for a head with position support + ?: return@flatMapLatest emptyFlow() + hmd.context.state.map { s -> + TrackerSnapshot( + // TODO: get HMD position from VR system once position is set in the tracker + position = Vector3.NULL, + rotation = s.rawRotation, + ) + } + } + + val controllerUpdates: Flow = serverContext.context.state + .flatMapLatest { state -> + val controllers = state.trackers.values.filter { + val bodyPart = it.context.state.value.bodyPart + bodyPart == BodyPart.LEFT_HAND || bodyPart == BodyPart.RIGHT_HAND + } + if (controllers.isEmpty()) return@flatMapLatest emptyFlow() + combine(controllers.map { controller -> + controller.context.state.map { s -> + // TODO: get controller position from tracker once position is set in the tracker + val position = Vector3.NULL + TrackerSnapshot(position = position, rotation = s.rawRotation) + } + }) { snapshots -> snapshots.minByOrNull { it.position.y }!! } + } + + fun start() { + sessionJob?.cancel() + sessionJob = context.scope.launch { runCalibrationSession(context, hmdUpdates, controllerUpdates) } + } + + fun cancel() { + sessionJob?.cancel() + sessionJob = null + context.dispatch(HeightCalibrationActions.Update(UserHeightCalibrationStatus.NONE, 0f)) + } + + companion object { + fun create( + serverContext: VRServer, + scope: CoroutineScope, + ): HeightCalibrationManager { + val behaviours = listOf(CalibrationBehaviour) + val context = Context.create( + initialState = INITIAL_HEIGHT_CALIBRATION_STATE, + scope = scope, + behaviours = behaviours, + ) + return HeightCalibrationManager(context = context, serverContext = serverContext) + } + } +} diff --git a/server/core/src/main/java/dev/slimevr/solarxr/heightcalibration.kt b/server/core/src/main/java/dev/slimevr/solarxr/heightcalibration.kt new file mode 100644 index 000000000..ff0786af4 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/solarxr/heightcalibration.kt @@ -0,0 +1,32 @@ +package dev.slimevr.solarxr + +import dev.slimevr.heightcalibration.HeightCalibrationManager +import kotlinx.coroutines.flow.drop +import kotlinx.coroutines.flow.launchIn +import kotlinx.coroutines.flow.onEach +import solarxr_protocol.rpc.CancelUserHeightCalibration +import solarxr_protocol.rpc.StartUserHeightCalibration +import solarxr_protocol.rpc.UserHeightRecordingStatusResponse + +class HeightCalibrationBehaviour( + private val heightCalibrationManager: HeightCalibrationManager, +) : SolarXRConnectionBehaviour { + override fun observe(receiver: SolarXRConnection) { + receiver.rpcDispatcher.on { + heightCalibrationManager.start() + } + + receiver.rpcDispatcher.on { + heightCalibrationManager.cancel() + } + + heightCalibrationManager.context.state.drop(1).onEach { state -> + receiver.sendRpc( + UserHeightRecordingStatusResponse( + status = state.status, + hmdheight = state.currentHeight, + ), + ) + }.launchIn(receiver.context.scope) + } +} diff --git a/server/core/src/main/java/dev/slimevr/solarxr/vrchat.kt b/server/core/src/main/java/dev/slimevr/solarxr/vrchat.kt index 4be41532d..53e184787 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/vrchat.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/vrchat.kt @@ -30,7 +30,7 @@ class VrcBehaviour( ) } - // Drop the initial value — we only want to push updates when the config changes + // Drop the initial value, we only want to push updates when the config changes vrcManager.context.state.drop(1).onEach { receiver.sendRpc(buildCurrentResponse()) }.launchIn(receiver.context.scope) diff --git a/server/core/src/test/java/dev/slimevr/heightcalibration/HeightCalibrationTest.kt b/server/core/src/test/java/dev/slimevr/heightcalibration/HeightCalibrationTest.kt new file mode 100644 index 000000000..ba87b0867 --- /dev/null +++ b/server/core/src/test/java/dev/slimevr/heightcalibration/HeightCalibrationTest.kt @@ -0,0 +1,358 @@ +@file:OptIn(kotlinx.coroutines.ExperimentalCoroutinesApi::class, kotlinx.coroutines.FlowPreview::class) + +package dev.slimevr.heightcalibration + +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import kotlinx.coroutines.Job +import kotlinx.coroutines.flow.MutableSharedFlow +import kotlinx.coroutines.launch +import kotlinx.coroutines.test.TestScope +import kotlinx.coroutines.test.advanceTimeBy +import kotlinx.coroutines.test.currentTime +import kotlinx.coroutines.test.runTest +import solarxr_protocol.rpc.UserHeightCalibrationStatus +import kotlin.test.Test +import kotlin.test.assertEquals + +// Stability durations in milliseconds (converted from nanosecond constants) +private const val FLOOR_STABILITY_MS = CONTROLLER_STABILITY_DURATION / 1_000_000L +private const val HEIGHT_STABILITY_MS = HEAD_STABILITY_DURATION / 1_000_000L + +// Rotation that maps controller forward (-Z) to down (-Y), satisfying the pointing-down check +private val POINTING_DOWN = Quaternion.fromTo(Vector3.NEG_Z, Vector3.NEG_Y) + +// Identity: controller forward is -Z, not pointing down +private val POINTING_FORWARD = Quaternion.IDENTITY + +// Identity: HMD up is +Y, within 15° leveled threshold +private val HMD_LEVEL = Quaternion.IDENTITY + +// 90° around Z: HMD up maps to +X, failing the leveled check +private val HMD_TILTED = Quaternion.fromTo(Vector3.POS_Y, Vector3.POS_X) + +// Position just below the floor threshold (0.10m) +private val FLOOR_POSITION = Vector3(0f, 0.05f, 0f) + +// Position comfortably above rise threshold (1.2m) with zero floor level +private val STANDING_POSITION = Vector3(0f, 1.7f, 0f) + +private fun makeContext(scope: kotlinx.coroutines.CoroutineScope) = HeightCalibrationContext.create( + initialState = INITIAL_HEIGHT_CALIBRATION_STATE, + scope = scope, + behaviours = listOf(CalibrationBehaviour), +) + +// Launches a calibration session with the virtual-time clock, so that advancing virtual time +// drives both the sample() operator and the stability duration checks in one unified timeline. +private fun TestScope.launchSession( + context: HeightCalibrationContext, + hmdFlow: MutableSharedFlow, + controllerFlow: MutableSharedFlow, +): Job { + val scope = this + return launch { + runCalibrationSession(context, hmdFlow, controllerFlow, clock = { scope.currentTime * 1_000_000L }) + } +} + +// Emits snapshot at SAMPLE_INTERVAL_MS rate for the given duration of virtual time, +// then does one extra advance to let any pending coroutine resumptions finish. +private suspend fun TestScope.emitFor( + flow: MutableSharedFlow, + snapshot: TrackerSnapshot, + durationMs: Long, +) { + val end = currentTime + durationMs + while (currentTime < end) { + flow.emit(snapshot) + advanceTimeBy(SAMPLE_INTERVAL_MS) + } + advanceTimeBy(SAMPLE_INTERVAL_MS) +} + +// Holds the controller steady on the floor long enough for the floor phase to complete. +private suspend fun TestScope.completeFloorPhase( + controllerFlow: MutableSharedFlow, + floorSnapshot: TrackerSnapshot, +) { + emitFor(controllerFlow, floorSnapshot, FLOOR_STABILITY_MS + SAMPLE_INTERVAL_MS * 5) +} + +// Holds the HMD steady at standing height long enough for the height phase to complete. +private suspend fun TestScope.completeHeightPhase( + hmdFlow: MutableSharedFlow, + hmdSnapshot: TrackerSnapshot, +) { + emitFor(hmdFlow, hmdSnapshot, HEIGHT_STABILITY_MS + SAMPLE_INTERVAL_MS * 5) +} + +class HeightCalibrationReducerTest { + @Test + fun `Update changes status and height`() = runTest { + val context = makeContext(this) + + context.dispatch(HeightCalibrationActions.Update(UserHeightCalibrationStatus.RECORDING_HEIGHT, 1.65f)) + + assertEquals(UserHeightCalibrationStatus.RECORDING_HEIGHT, context.state.value.status) + assertEquals(1.65f, context.state.value.currentHeight) + } +} + +class HeightCalibrationSessionTest { + @Test + fun `session starts in RECORDING_FLOOR`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + assertEquals(UserHeightCalibrationStatus.RECORDING_FLOOR, context.state.value.status) + job.cancel() + } + + @Test + fun `controller too high does not change status`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + emitFor(controllerFlow, TrackerSnapshot(Vector3(0f, 0.5f, 0f), POINTING_DOWN), SAMPLE_INTERVAL_MS) + + assertEquals(UserHeightCalibrationStatus.RECORDING_FLOOR, context.state.value.status) + job.cancel() + } + + @Test + fun `controller not pointing down transitions to WAITING_FOR_CONTROLLER_PITCH`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + emitFor(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_FORWARD), SAMPLE_INTERVAL_MS) + + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_CONTROLLER_PITCH, context.state.value.status) + job.cancel() + } + + @Test + fun `stable floor transitions to WAITING_FOR_RISE`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_RISE, context.state.value.status) + job.cancel() + } + + @Test + fun `HMD below rise threshold stays WAITING_FOR_RISE`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + + emitFor(hmdFlow, TrackerSnapshot(Vector3(0f, 0.5f, 0f), HMD_LEVEL), SAMPLE_INTERVAL_MS) + + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_RISE, context.state.value.status) + job.cancel() + } + + @Test + fun `HMD not leveled transitions to WAITING_FOR_FW_LOOK`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + + emitFor(hmdFlow, TrackerSnapshot(STANDING_POSITION, HMD_TILTED), SAMPLE_INTERVAL_MS) + + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_FW_LOOK, context.state.value.status) + job.cancel() + } + + @Test + fun `stable HMD at valid height transitions to DONE`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + completeHeightPhase(hmdFlow, TrackerSnapshot(STANDING_POSITION, HMD_LEVEL)) + + assertEquals(UserHeightCalibrationStatus.DONE, context.state.value.status) + assertEquals(STANDING_POSITION.y - FLOOR_POSITION.y, context.state.value.currentHeight) + job.cancel() + } + + @Test + fun `stable HMD below HEIGHT_MIN transitions to ERROR_TOO_SMALL`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(Vector3(0f, 0f, 0f), POINTING_DOWN)) + completeHeightPhase(hmdFlow, TrackerSnapshot(Vector3(0f, 1.3f, 0f), HMD_LEVEL)) + + assertEquals(UserHeightCalibrationStatus.ERROR_TOO_SMALL, context.state.value.status) + job.cancel() + } + + @Test + fun `stable HMD above HEIGHT_MAX transitions to ERROR_TOO_HIGH`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(Vector3(0f, 0f, 0f), POINTING_DOWN)) + completeHeightPhase(hmdFlow, TrackerSnapshot(Vector3(0f, 2.0f, 0f), HMD_LEVEL)) + + assertEquals(UserHeightCalibrationStatus.ERROR_TOO_HIGH, context.state.value.status) + job.cancel() + } + + @Test + fun `unstable floor sample resets controller stability window`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + val stableSnapshot = TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN) + // X offset of 1m pushes energy well above CONTROLLER_STABILITY_THRESHOLD + val unstableSnapshot = TrackerSnapshot(Vector3(1f, FLOOR_POSITION.y, 0f), POINTING_DOWN) + + // Build up stability but not long enough to complete + emitFor(controllerFlow, stableSnapshot, FLOOR_STABILITY_MS - SAMPLE_INTERVAL_MS * 5) + assertEquals(UserHeightCalibrationStatus.RECORDING_FLOOR, context.state.value.status) + + // Unstable sample resets the stability window + emitFor(controllerFlow, unstableSnapshot, SAMPLE_INTERVAL_MS) + assertEquals(UserHeightCalibrationStatus.RECORDING_FLOOR, context.state.value.status) + + // Must hold stable for the full duration again from the reset point + completeFloorPhase(controllerFlow, stableSnapshot) + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_RISE, context.state.value.status) + job.cancel() + } + + @Test + fun `out-of-threshold HMD sample resets height stability window`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + + val stableSnapshot = TrackerSnapshot(STANDING_POSITION, HMD_LEVEL) + val unstableSnapshot = TrackerSnapshot(Vector3(0f, 1.9f, 0f), HMD_LEVEL) + + // Build up stability but not long enough to complete + emitFor(hmdFlow, stableSnapshot, HEIGHT_STABILITY_MS - SAMPLE_INTERVAL_MS * 5) + assertEquals(UserHeightCalibrationStatus.RECORDING_HEIGHT, context.state.value.status) + + // Unstable sample resets the stability window + emitFor(hmdFlow, unstableSnapshot, SAMPLE_INTERVAL_MS) + assertEquals(UserHeightCalibrationStatus.RECORDING_HEIGHT, context.state.value.status) + + // Must hold stable for the full duration again from the reset point + completeHeightPhase(hmdFlow, stableSnapshot) + assertEquals(UserHeightCalibrationStatus.DONE, context.state.value.status) + job.cancel() + } + + @Test + fun `timeout fires ERROR_TIMEOUT`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launch { runCalibrationSession(context, hmdFlow, controllerFlow) } + + advanceTimeBy(TIMEOUT_MS + 1) + + assertEquals(UserHeightCalibrationStatus.ERROR_TIMEOUT, context.state.value.status) + job.cancel() + } + + @Test + fun `controller pitch recovery leads to WAITING_FOR_RISE`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + // Bad pitch triggers WAITING_FOR_CONTROLLER_PITCH + emitFor(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_FORWARD), SAMPLE_INTERVAL_MS) + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_CONTROLLER_PITCH, context.state.value.status) + + // Recovery: hold steady on floor for the required duration + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_RISE, context.state.value.status) + job.cancel() + } + + @Test + fun `leveling HMD after WAITING_FOR_FW_LOOK transitions to RECORDING_HEIGHT`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + + // Tilted HMD + emitFor(hmdFlow, TrackerSnapshot(STANDING_POSITION, HMD_TILTED), SAMPLE_INTERVAL_MS) + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_FW_LOOK, context.state.value.status) + + // Recovery: level the HMD + emitFor(hmdFlow, TrackerSnapshot(STANDING_POSITION, HMD_LEVEL), SAMPLE_INTERVAL_MS) + assertEquals(UserHeightCalibrationStatus.RECORDING_HEIGHT, context.state.value.status) + job.cancel() + } + + @Test + fun `HMD rising above threshold transitions to RECORDING_HEIGHT`() = runTest { + val context = makeContext(this) + val controllerFlow = MutableSharedFlow(extraBufferCapacity = 1) + val hmdFlow = MutableSharedFlow(extraBufferCapacity = 1) + val job = launchSession(context, hmdFlow, controllerFlow) + advanceTimeBy(SAMPLE_INTERVAL_MS) + + completeFloorPhase(controllerFlow, TrackerSnapshot(FLOOR_POSITION, POINTING_DOWN)) + + // HMD below threshold + emitFor(hmdFlow, TrackerSnapshot(Vector3(0f, 0.5f, 0f), HMD_LEVEL), SAMPLE_INTERVAL_MS) + assertEquals(UserHeightCalibrationStatus.WAITING_FOR_RISE, context.state.value.status) + + // HMD rises above threshold + emitFor(hmdFlow, TrackerSnapshot(STANDING_POSITION, HMD_LEVEL), SAMPLE_INTERVAL_MS) + assertEquals(UserHeightCalibrationStatus.RECORDING_HEIGHT, context.state.value.status) + job.cancel() + } +} 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 ca2901530..ca454883d 100644 --- a/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt +++ b/server/desktop/src/main/java/dev/slimevr/desktop/Main.kt @@ -9,9 +9,11 @@ import dev.slimevr.desktop.ipc.createIpcServers import dev.slimevr.desktop.serial.createDesktopSerialServer import dev.slimevr.desktop.vrchat.createDesktopVRCConfigManager import dev.slimevr.firmware.FirmwareManager +import dev.slimevr.heightcalibration.HeightCalibrationManager import dev.slimevr.resolveConfigDirectory import dev.slimevr.solarxr.DataFeedInitBehaviour import dev.slimevr.solarxr.FirmwareBehaviour +import dev.slimevr.solarxr.HeightCalibrationBehaviour import dev.slimevr.solarxr.SerialBehaviour import dev.slimevr.solarxr.VrcBehaviour import dev.slimevr.solarxr.createSolarXRWebsocketServer @@ -28,6 +30,7 @@ fun main(args: Array) = runBlocking { val firmwareManager = FirmwareManager.create(serialServer = serialServer, scope = this) val vrcConfigManager = createDesktopVRCConfigManager(config = config, scope = this) + val heightCalibrationManager = HeightCalibrationManager.create(serverContext = server, scope = this) launch { createUDPTrackerServer(server, config) @@ -41,6 +44,7 @@ fun main(args: Array) = runBlocking { SerialBehaviour(serialServer), FirmwareBehaviour(firmwareManager), VrcBehaviour(vrcConfigManager, userHeight = { config.userConfig.context.state.value.data.userHeight.toDouble() }), + HeightCalibrationBehaviour(heightCalibrationManager), ) launch { createSolarXRWebsocketServer(server, solarXRBehaviours) } launch { createIpcServers(server, solarXRBehaviours) }