From c56d95351630c30baad64198f60ca010baeb9665 Mon Sep 17 00:00:00 2001 From: jabberrock <130935387+jabberrock@users.noreply.github.com> Date: Wed, 21 May 2025 12:34:20 -0700 Subject: [PATCH] [Stay Aligned] Select yaw correction rate based on IMU type (#1438) --- .../dev/slimevr/config/StayAlignedConfig.kt | 10 ----- .../rpc/settings/RPCSettingsBuilderKotlin.kt | 2 +- .../rpc/settings/RPCSettingsHandler.kt | 1 - .../processor/stayaligned/StayAligned.kt | 12 +++--- .../stayaligned/StayAlignedDefaults.kt | 38 ++++++++++++++++--- 5 files changed, 40 insertions(+), 23 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/config/StayAlignedConfig.kt b/server/core/src/main/java/dev/slimevr/config/StayAlignedConfig.kt index 29a9ee3ac..318223dbb 100644 --- a/server/core/src/main/java/dev/slimevr/config/StayAlignedConfig.kt +++ b/server/core/src/main/java/dev/slimevr/config/StayAlignedConfig.kt @@ -9,16 +9,6 @@ class StayAlignedConfig { */ var enabled = false - /** - * Applies extra yaw correction to support worse IMUs - * - * We could let players choose a yaw correction amount instead, but this lead to - * players agonizing about choosing the "right" yaw correction amount. In practice, - * we only need 2 yaw correction amounts - a default one for most IMUs, and an extra - * one for terrible IMUs. - */ - var extraYawCorrection = false - /** * Temporarily hide the yaw correction from Stay Aligned. * diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilderKotlin.kt b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilderKotlin.kt index 8a280e76d..a34322c6f 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilderKotlin.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilderKotlin.kt @@ -14,7 +14,7 @@ object RPCSettingsBuilderKotlin { .createStayAlignedSettings( fbb, config.enabled, - config.extraYawCorrection, + false, // deprecated config.hideYawCorrection, config.standingRelaxedPose.enabled, config.standingRelaxedPose.upperLegAngleInDeg, diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt index 0ba695394..149aa8553 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt @@ -353,7 +353,6 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) { val config = api.server.configManager.vrConfig.stayAlignedConfig val requestConfig = req.stayAligned() config.enabled = requestConfig.enabled() - config.extraYawCorrection = requestConfig.extraYawCorrection() config.hideYawCorrection = requestConfig.hideYawCorrection() config.standingRelaxedPose.enabled = requestConfig.standingEnabled() config.standingRelaxedPose.upperLegAngleInDeg = requestConfig.standingUpperLegAngle() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAligned.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAligned.kt index e5b4a9ad6..e31d0e7fd 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAligned.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAligned.kt @@ -2,8 +2,9 @@ package dev.slimevr.tracking.processor.stayaligned import dev.slimevr.VRServer import dev.slimevr.config.StayAlignedConfig -import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.EXTRA_YAW_CORRECTION_PER_SEC -import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.YAW_CORRECTION_PER_SEC +import dev.slimevr.math.Angle +import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.IMU_TO_YAW_CORRECTION +import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.YAW_CORRECTION_DEFAULT import dev.slimevr.tracking.processor.stayaligned.adjust.AdjustTrackerYaw import dev.slimevr.tracking.processor.stayaligned.trackers.TrackerSkeleton @@ -37,9 +38,10 @@ object StayAligned { // Update hide correction since the config could have changed trackerToAdjust.stayAligned.hideCorrection = config.hideYawCorrection - var yawCorrectionPerSec = YAW_CORRECTION_PER_SEC - if (config.extraYawCorrection) { - yawCorrectionPerSec += EXTRA_YAW_CORRECTION_PER_SEC + val yawCorrectionPerSec = + IMU_TO_YAW_CORRECTION.getOrDefault(trackerToAdjust.imuType, YAW_CORRECTION_DEFAULT) + if (yawCorrectionPerSec == Angle.ZERO) { + return } // Scale yaw correction since we're only updating one tracker per tick diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAlignedDefaults.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAlignedDefaults.kt index 08fbae425..3d350ae0f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAlignedDefaults.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAlignedDefaults.kt @@ -3,6 +3,7 @@ package dev.slimevr.tracking.processor.stayaligned import dev.slimevr.math.Angle import dev.slimevr.tracking.processor.stayaligned.poses.RelaxedPose import dev.slimevr.tracking.processor.stayaligned.trackers.RestDetector +import dev.slimevr.tracking.trackers.udp.IMUType import kotlin.time.Duration.Companion.seconds /** @@ -11,12 +12,6 @@ import kotlin.time.Duration.Companion.seconds */ object StayAlignedDefaults { - // Maximum yaw correction to apply - val YAW_CORRECTION_PER_SEC = Angle.ofDeg(0.20f) - - // Extra yaw correction to apply to terrible IMUs - val EXTRA_YAW_CORRECTION_PER_SEC = Angle.ofDeg(0.20f) - // Rest detector for detecting when trackers are at rest fun makeRestDetector() = RestDetector( @@ -44,4 +39,35 @@ object StayAlignedDefaults { const val YAW_ERRORS_LOCKED_ERROR_WEIGHT = 10.0f const val YAW_ERRORS_CENTER_ERROR_WEIGHT = 2.0f const val YAW_ERRORS_NEIGHBOR_ERROR_WEIGHT = 1.0f + + // Yaw correction for each type of IMU + val YAW_CORRECTION_IMU_GOOD = Angle.ofDeg(0.1f) + val YAW_CORRECTION_IMU_OK = Angle.ofDeg(0.2f) + val YAW_CORRECTION_IMU_BAD = Angle.ofDeg(0.4f) + val YAW_CORRECTION_IMU_DISABLED = Angle.ZERO + + val IMU_TO_YAW_CORRECTION = buildMap { + // Mag is enabled on MPU9250 but server doesn't know about it + set(IMUType.MPU9250, YAW_CORRECTION_IMU_DISABLED) + set(IMUType.MPU6500, YAW_CORRECTION_IMU_BAD) + set(IMUType.BNO080, YAW_CORRECTION_IMU_GOOD) + set(IMUType.BNO085, YAW_CORRECTION_IMU_GOOD) + set(IMUType.BNO055, YAW_CORRECTION_IMU_BAD) + set(IMUType.MPU6050, YAW_CORRECTION_IMU_BAD) + set(IMUType.BNO086, YAW_CORRECTION_IMU_GOOD) + set(IMUType.BMI160, YAW_CORRECTION_IMU_BAD) + set(IMUType.ICM20948, YAW_CORRECTION_IMU_BAD) + set(IMUType.ICM42688, YAW_CORRECTION_IMU_OK) + set(IMUType.BMI270, YAW_CORRECTION_IMU_OK) + set(IMUType.LSM6DS3TRC, YAW_CORRECTION_IMU_BAD) + set(IMUType.LSM6DSV, YAW_CORRECTION_IMU_GOOD) + set(IMUType.LSM6DSO, YAW_CORRECTION_IMU_OK) + set(IMUType.LSM6DSR, YAW_CORRECTION_IMU_GOOD) + set(IMUType.ICM45686, YAW_CORRECTION_IMU_GOOD) + set(IMUType.ICM45605, YAW_CORRECTION_IMU_GOOD) + } + + // Assume any new IMUs are at least OK, or else we wouldn't be writing firmware to + // support it. Please classify and add new IMUs to the map above! + val YAW_CORRECTION_DEFAULT = YAW_CORRECTION_IMU_OK }