Fix trackerassignment and add default mountingOrientation

This commit is contained in:
Erimel committed 2026-07-17 15:33:53 -04:00
1 parent 349666b8cd
commit bf6d8eb50d
6 files changed
+67 -21

No files matched your search

@@ -37,7 +37,7 @@ data class TrackersConfig(
@Serializable
data class HidConfig(
val trackersOverHid: Boolean = false,
val trackersOverHid: Boolean = false, // TODO
)
@Serializable
@@ -48,7 +48,7 @@ data class OutputTrackersConfig(
*/
@Serializable(with = BodyPartListSerializer::class)
val trackers: List<BodyPart> = listOf(),
val sendDerivedVelocity: Boolean = false,
val sendDerivedVelocity: Boolean = false, // TODO
)
@Serializable
@@ -81,14 +81,14 @@ data class ResetsConfig(
/** Always reset mounting for feet */
val resetMountingFeet: Boolean = false,
/** Reset mode used for the arms */
val armsResetMode: ArmsResetMode = ArmsResetMode.BACK,
val armsResetMode: ArmsResetMode = ArmsResetMode.BACK, // TODO
/** Yaw reset smoothing time in seconds */
val yawResetSmoothTime: Float = 0.0f,
val yawResetSmoothTime: Float = 0.0f, // TODO
/** Save automatic mounting reset calibration */
val saveMountingReset: Boolean = false,
val saveMountingReset: Boolean = false, // TODO
/** Reset the HMD's pitch upon full reset */
val resetHmdPitch: Boolean = false,
val lastMountingMethod: MountingMethods = MountingMethods.AUTOMATIC,
val resetHmdPitch: Boolean = false, // TODO
val lastMountingMethod: MountingMethods = MountingMethods.AUTOMATIC, // TODO
)
// Used in SkeletonConfig
@@ -109,8 +109,7 @@ class HIDDeviceInfoBehaviour : HIDReceiverBehaviour {
}
receiver.context.dispatch(HIDReceiverActions.TrackerRegistered(packet.hidId, tracker.context.state.value.id))
// HID does not have a rest calibration signal
tracker.context.dispatch(TrackerActions.Update { copy(sensorType = packet.imuType, completedRestCalibration = true, magStatus = packet.magStatus) })
tracker.context.dispatch(TrackerActions.SetStatus(TrackerStatus.OK))
tracker.context.dispatch(TrackerActions.Update { copy(sensorType = packet.imuType, completedRestCalibration = true, magStatus = packet.magStatus, status = TrackerStatus.OK) })
}
}
}
@@ -21,11 +21,17 @@ class AssignTrackerBehaviour(
TrackerActions.Update {
copy(
bodyPart = bodyPart,
customName = req.displayName,
mountingOrientation = mountingOrientation,
customName = req.displayName ?: customName,
)
},
)
// Override default mounting orientation set from changing the bodyPart
if (mountingOrientation != null) {
tracker.context.dispatch(
TrackerActions.SetMountingOrientation(mountingOrientation),
)
}
}
}
}
@@ -11,6 +11,7 @@ import kotlinx.coroutines.flow.launchIn
import kotlinx.coroutines.flow.map
import kotlinx.coroutines.flow.onEach
import kotlinx.coroutines.isActive
import solarxr_protocol.datatypes.BodyPart
import kotlin.concurrent.atomics.AtomicInt
import kotlin.concurrent.atomics.ExperimentalAtomicApi
import kotlin.concurrent.atomics.incrementAndFetch
@@ -67,6 +68,8 @@ class TrackerBasicBehaviour : TrackerBehaviour {
)
}
is TrackerActions.SetMountingOrientation -> state.copy(mountingOrientation = action.mountingOrientation)
is TrackerActions.FullReset -> {
val headingCorrection = estimateHeadingCorrect(
state.rawRotation,
@@ -129,6 +132,48 @@ class TrackerBasicBehaviour : TrackerBehaviour {
}
}
class TrackerDefaultMountingOrientationBehaviour : TrackerBehaviour {
/**
* Returns the default mounting orientation for the body part
*/
private fun defaultMountingForBodyPart(bodyPart: BodyPart?): Quaternion = when (bodyPart) {
BodyPart.LEFT_LOWER_ARM, BodyPart.LEFT_HAND,
BodyPart.LEFT_INDEX_PROXIMAL, BodyPart.LEFT_INDEX_INTERMEDIATE,
BodyPart.LEFT_INDEX_DISTAL, BodyPart.LEFT_MIDDLE_PROXIMAL,
BodyPart.LEFT_MIDDLE_INTERMEDIATE, BodyPart.LEFT_MIDDLE_DISTAL,
BodyPart.LEFT_RING_PROXIMAL, BodyPart.LEFT_RING_INTERMEDIATE,
BodyPart.LEFT_RING_DISTAL, BodyPart.LEFT_LITTLE_PROXIMAL,
BodyPart.LEFT_LITTLE_INTERMEDIATE, BodyPart.LEFT_LITTLE_DISTAL,
BodyPart.LEFT_SHOULDER,
-> Quaternion.SLIMEVR.LEFT
BodyPart.RIGHT_LOWER_ARM, BodyPart.RIGHT_HAND,
BodyPart.RIGHT_INDEX_PROXIMAL, BodyPart.RIGHT_INDEX_INTERMEDIATE,
BodyPart.RIGHT_INDEX_DISTAL, BodyPart.RIGHT_MIDDLE_PROXIMAL,
BodyPart.RIGHT_MIDDLE_INTERMEDIATE, BodyPart.RIGHT_MIDDLE_DISTAL,
BodyPart.RIGHT_RING_PROXIMAL, BodyPart.RIGHT_RING_INTERMEDIATE,
BodyPart.RIGHT_RING_DISTAL, BodyPart.RIGHT_LITTLE_PROXIMAL,
BodyPart.RIGHT_LITTLE_INTERMEDIATE, BodyPart.RIGHT_LITTLE_DISTAL,
BodyPart.RIGHT_SHOULDER,
-> Quaternion.SLIMEVR.RIGHT
BodyPart.LEFT_UPPER_ARM, BodyPart.LEFT_LOWER_LEG -> Quaternion.SLIMEVR.FRONT_LEFT
BodyPart.RIGHT_UPPER_ARM, BodyPart.RIGHT_LOWER_LEG -> Quaternion.SLIMEVR.FRONT_RIGHT
else -> Quaternion.SLIMEVR.FRONT
}
override fun observe(receiver: Tracker) {
receiver.context.state
.map { it.bodyPart }
.distinctUntilChanged()
.onEach {
receiver.context.dispatch(TrackerActions.SetMountingOrientation(defaultMountingForBodyPart(it)))
}.launchIn(receiver.context.scope)
}
}
class TrackerTPSBehaviour : TrackerBehaviour {
@OptIn(ExperimentalAtomicApi::class)
override fun observe(receiver: Tracker) {
@@ -45,6 +45,7 @@ sealed interface TrackerActions {
data class SetMagStatus(val status: MagnetometerStatus) : TrackerActions
data class SetStatus(val status: TrackerStatus) : TrackerActions
data class SetRotation(val rotation: Quaternion? = null, val acceleration: Vector3? = null, val magnetometer: Vector3? = null) : TrackerActions
data class SetMountingOrientation(val mountingOrientation: HeadingAlignment?) : TrackerActions
data class FullReset(val referenceRotation: Quaternion) : TrackerActions
data class YawReset(val referenceRotation: Quaternion) : TrackerActions
data class MountingReset(val referenceRotation: Quaternion) : TrackerActions
@@ -104,6 +105,7 @@ class Tracker(
val behaviours = listOf(
TrackerBasicBehaviour(),
TrackerDefaultMountingOrientationBehaviour(),
TrackerConfigBehaviour(settings, hardwareId),
TrackerTPSBehaviour(),
TrackerToSkeletonBehaviour(),
@@ -334,21 +334,15 @@ class SensorInfoBehaviour : UDPConnectionBehaviour {
val existingTracker = receiver.getTracker(event.data.sensorId)
if (existingTracker != null) {
existingTracker.context.dispatchAll(
listOf(
TrackerActions.Update { copy(sensorType = event.data.imuType, completedRestCalibration = event.data.hasCompletedRestCalibration) },
TrackerActions.SetStatus(event.data.status),
),
existingTracker.context.dispatch(
TrackerActions.Update { copy(sensorType = event.data.imuType, completedRestCalibration = event.data.hasCompletedRestCalibration, status = event.data.status) },
)
return@onPacket
}
val (tracker, isNew) = assignTracker(receiver, device, event)
tracker.context.dispatchAll(
listOf(
TrackerActions.Update { copy(sensorType = event.data.imuType, completedRestCalibration = event.data.hasCompletedRestCalibration) },
TrackerActions.SetStatus(event.data.status),
),
tracker.context.dispatch(
TrackerActions.Update { copy(sensorType = event.data.imuType, completedRestCalibration = event.data.hasCompletedRestCalibration, status = event.data.status) },
)
if (isNew && tracker.context.state.value.magStatus == MagnetometerStatus.NOT_SUPPORTED) {
tracker.context.dispatch(