Add Velocity calculation logic and GUI toggle (end-to-end MVP). (#1753)

This commit is contained in:
M1DNYT3 authored and GitHub committed 2026-05-04 15:04:53 +02:00
1 parent dfedf99b33
commit 065e774309
11 files changed
+601 -40

No files matched your search

@@ -58,6 +58,8 @@ class VRConfig {
val trackingChecklist: TrackingChecklistConfig = TrackingChecklistConfig()
val velocityConfig: VelocityConfig = VelocityConfig()
val vrcConfig: VRCConfig = VRCConfig()
init {
@@ -119,6 +121,7 @@ class VRConfig {
.readFilteringConfig(filters, tracker.getRotation())
}
}
tracker.allowVelocity = velocityConfig.sendDerivedVelocity
}
fun writeTrackerConfig(tracker: Tracker?) {
@@ -0,0 +1,19 @@
package dev.slimevr.config
import dev.slimevr.VRServer
/**
* Allows to enable/disable sending of optional derived velocity data via Protobuf.
* Enables Natural Locomotion Support
* May create overprediction in certain titles causing excessive jitter when moving upper body.
*/
class VelocityConfig {
// Disables derived velocity for all trackers. Driver zeroes out velocity if nothing is returned in protobuf message.
var sendDerivedVelocity: Boolean = false
fun updateTrackersVelocitySettings() {
for (t in VRServer.instance.allTrackers) {
t.allowVelocity = sendDerivedVelocity
}
}
}
@@ -15,6 +15,7 @@ import dev.slimevr.config.StayAlignedConfig
import dev.slimevr.config.TapDetectionConfig
import dev.slimevr.config.VMCConfig
import dev.slimevr.config.VRCOSCConfig
import dev.slimevr.config.VelocityConfig
import dev.slimevr.filtering.TrackerFilters.Companion.getByConfigkey
import dev.slimevr.tracking.processor.HumanPoseManager
import dev.slimevr.tracking.processor.config.SkeletonConfigToggles
@@ -34,6 +35,7 @@ import solarxr_protocol.rpc.SteamVRTrackersSetting
import solarxr_protocol.rpc.TapDetectionSettings
import solarxr_protocol.rpc.VMCOSCSettings
import solarxr_protocol.rpc.VRCOSCSettings
import solarxr_protocol.rpc.VelocitySettings
import solarxr_protocol.rpc.settings.LegTweaksSettings
import solarxr_protocol.rpc.settings.ModelRatios
import solarxr_protocol.rpc.settings.ModelSettings
@@ -427,6 +429,7 @@ fun createSettingsResponse(fbb: FlatBufferBuilder, server: VRServer): Int {
),
createHIDSettings(fbb, server.configManager.vrConfig.hidConfig),
0,
createVelocitySettings(fbb, server.configManager.vrConfig.velocityConfig),
)
}
@@ -462,3 +465,12 @@ fun createHIDSettings(
fbb,
config.trackersOverHID,
)
fun createVelocitySettings(
fbb: FlatBufferBuilder,
config: VelocityConfig,
): Int = VelocitySettings
.createVelocitySettings(
fbb,
config.sendDerivedVelocity,
)
@@ -367,6 +367,12 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
config.trackersOverHID = requestConfig.trackersOverHid()
}
if (req.velocitySettings() != null) {
val velocityConfig = api.server.configManager.vrConfig.velocityConfig
velocityConfig.sendDerivedVelocity = req.velocitySettings().sendDerivedVelocity()
velocityConfig.updateTrackersVelocitySettings()
}
api.server.configManager.saveConfig()
}
@@ -384,7 +390,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
val settings = SettingsResponse
.createSettingsResponse(
fbb,
createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
)
val outbound =
rpcHandler.createRPCMessage(fbb, RpcMessage.SettingsResponse, settings)
@@ -1171,6 +1171,7 @@ class HumanSkeleton(
it.position = trackerBone.getTailPosition()
it.setRotation(trackerBone.getGlobalRotation() * trackerBone.rotationOffset.inv())
it.dataTick()
it.updateDerivedVelocity()
}
}
@@ -12,6 +12,10 @@ import io.eiren.util.BufferedTimer
import io.github.axisangles.ktmath.Quaternion
import io.github.axisangles.ktmath.Vector3
import kotlin.properties.Delegates
import kotlin.time.Duration.Companion.microseconds
import kotlin.time.Duration.Companion.milliseconds
import kotlin.time.DurationUnit
import kotlin.time.TimeSource
const val TIMEOUT_MS = 2_000L
const val DISCONNECT_MS = 3_000L + TIMEOUT_MS
@@ -75,6 +79,11 @@ class Tracker @JvmOverloads constructor(
*/
val allowMounting: Boolean = false,
/**
* If true, the tracker will send Derived Velocity.
*/
var allowVelocity: Boolean = false,
val isHmd: Boolean = false,
/**
@@ -111,7 +120,19 @@ class Tracker @JvmOverloads constructor(
// IMU: +z forward, +x left, +y up
// SlimeVR: +z backward, +x right, +y up
private var _acceleration = Vector3.NULL
private var _velocity = Vector3.NULL
private var _magVector = Vector3.NULL
/**
* Velocity state server-side differentiation based on sent poses
*/
private data class VelocityState(
var prevMark: TimeSource.Monotonic.ValueTimeMark? = null,
var prevPos: Vector3 = Vector3(0f, 0f, 0f),
)
private val velocityState = VelocityState()
var position = Vector3.NULL
val resetsHandler: TrackerResetsHandler = TrackerResetsHandler(this)
val filteringHandler: TrackerFilteringHandler = TrackerFilteringHandler()
@@ -345,6 +366,39 @@ class Tracker @JvmOverloads constructor(
}
}
/**
* If derived velocity is enabled, updates the derived velocity of the tracker,
* calculating it based on the position displacement since the last update,
* applying a sanity check on the time delta to filter out noise and ensure data stability.
*/
fun updateDerivedVelocity() {
if (!allowVelocity || !hasPosition) {
velocityState.prevMark = null
_velocity = Vector3.NULL
return
}
val pos = position
val now = TimeSource.Monotonic.markNow()
val prevMark = velocityState.prevMark
if (prevMark != null) {
val dt = now - prevMark
if (dt in 100.microseconds..250.milliseconds) {
val dtSeconds = dt.toDouble(DurationUnit.SECONDS)
_velocity = Vector3(
((pos.x - velocityState.prevPos.x) / dtSeconds).toFloat(),
((pos.y - velocityState.prevPos.y) / dtSeconds).toFloat(),
((pos.z - velocityState.prevPos.z) / dtSeconds).toFloat(),
)
} else {
_velocity = Vector3.NULL
}
}
velocityState.prevMark = now
velocityState.prevPos = pos
}
/**
* Gets the identity-adjusted tracker rotation after the resetsHandler's corrections
* (identity reset, drift and identity mounting).
@@ -421,6 +475,24 @@ class Tracker @JvmOverloads constructor(
this._acceleration = vec
}
/**
* Sets the derived velocity of the tracker.
*/
fun setVelocity(vec: Vector3) {
this._velocity = if (allowVelocity) vec else Vector3.NULL
}
/**
* Gets the derived velocity of the tracker.
*/
fun getVelocity(): Vector3 = _velocity
/**
* True if the tracker has valid velocity data.
*/
val hasVelocity: Boolean
get() = allowVelocity && _velocity != Vector3.NULL
/**
* True if the raw rotation is coming directly from an IMU (no cameras or lighthouses)
* For example, flex sensor trackers are not considered as IMU trackers (see TrackerDataType)