update todo comment

.github/CODEOWNERS

@@ -2,7 +2,7 @@
 * @Eirenliel
 
 # Make everyone be able to approve SolarXR submodule changes
-/solarxr-protocol @ButterscotchV @Louka3000 @ImUrX
+/solarxr-protocol @ButterscotchV @Erimelowo @ImUrX
 
 # Make Loucas and Uriel the owners of all GUI stuff
 /gui/ @ImUrX
@@ -10,26 +10,26 @@
 /pnpm-workspace.yaml @ImUrX
 
 # Uriel and Erimel responsible for i18n
-/gui/public/i18n/ @ImUrX @Louka3000
-/gui/src/i18n/ @ImUrX @Louka3000
-/l10n.toml @ImUrX @Louka3000
+/gui/public/i18n/ @ImUrX @Erimelowo
+/gui/src/i18n/ @ImUrX @Erimelowo
+/l10n.toml @ImUrX @Erimelowo
 
-/gui/src/components/settings/ @Louka3000 @ImUrX
+/gui/src/components/settings/ @Erimelowo @ImUrX
 
 # Rust part of the GUI
 /gui/src-tauri/ @ImUrX
 /Cargo.lock @ImUrX
 
 # Some server code~
-/server/ @ButterscotchV @Eirenliel @Louka3000
+/server/ @ButterscotchV @Eirenliel @Erimelowo
 
 /server/src/main/java/dev/slimevr/autobone/ @ButterscotchV
 /server/src/main/java/dev/slimevr/poserecorder/ @ButterscotchV
 /server/src/main/java/dev/slimevr/posestreamer/ @ButterscotchV
 
-/server/src/main/java/dev/slimevr/osc/ @Louka3000
-/server/src/main/java/dev/slimevr/tracking/processor/ @Louka3000
-/server/src/main/java/dev/slimevr/filtering/ @Louka3000
+/server/src/main/java/dev/slimevr/osc/ @Erimelowo
+/server/src/main/java/dev/slimevr/tracking/processor/ @Erimelowo
+/server/src/main/java/dev/slimevr/filtering/ @Erimelowo
 
 # Linux files
 *.nix @ImUrX

.github/workflows/gradle.yaml

@@ -255,7 +255,7 @@ jobs:
 
       - name: Build
         run: |
-          rustup target add aarch64-apple-darwin
+          rustup target add x86_64-apple-darwin
           pnpm i
           pnpm run tauri build --target universal-apple-darwin
 

gui/public/i18n/en/translation.ftl

@@ -44,6 +44,7 @@ body_part-LEFT_HAND = Left hand
 body_part-LEFT_UPPER_LEG = Left thigh
 body_part-LEFT_LOWER_LEG = Left ankle
 body_part-LEFT_FOOT = Left foot
+body_part-PLAYSPACE = Playspace (motion compensation)
 
 ## Proportions
 skeleton_bone-NONE = None

gui/src/components/commons/BodyPartIcon.tsx

@@ -86,6 +86,9 @@ export const mapPart: Record<
     <UpperLegIcon width={width} flipped></UpperLegIcon>
   ),
   [BodyPart.WAIST]: ({ width }) => <WaistIcon width={width}></WaistIcon>,
+  [BodyPart.PLAYSPACE]: ({ width }) => (
+    <SlimeVRIcon width={width}></SlimeVRIcon>
+  ), // TODO
 };
 
 export function BodyPartIcon({

gui/src/components/onboarding/BodyAssignment.tsx

@@ -307,6 +307,16 @@ export function BodyAssignment({
                 onClick={() => onRoleSelected(SIDES[right].foot)}
                 direction="left"
               />
+              {advanced && (
+                <TrackerPartCard
+                  onlyAssigned={onlyAssigned}
+                  roleError={rolesWithErrors[BodyPart.PLAYSPACE]?.label}
+                  td={trackerPartGrouped[BodyPart.PLAYSPACE]}
+                  role={BodyPart.PLAYSPACE}
+                  onClick={() => onRoleSelected(BodyPart.PLAYSPACE)}
+                  direction="left"
+                />
+              )}
             </div>
           </div>
         }

gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx

@@ -99,6 +99,7 @@ export const mapPart: Record<
       <FootIcon width={width} flipped></FootIcon>
     ),
   [BodyPart.WAIST]: ({ width }) => <FootIcon width={width}></FootIcon>,
+  [BodyPart.PLAYSPACE]: ({ width }) => <FootIcon width={width}></FootIcon>,
 };
 
 export function MountingBodyPartIcon({

gui/src/utils/a11y.ts

@@ -15,7 +15,7 @@ export function waitUntil(
   tries?: number
 ): Promise<void> {
   return new Promise((resolve, rej) => {
-    const isPromise = typeof condition() === 'boolean';
+    const isPromise = typeof condition() !== 'boolean';
     const interval = setInterval(() => {
       if (tries && --tries === 0) {
         error(new Error('waitUntil ran out of tries'));

gui/src/utils/skeletonHelper.ts

@@ -185,6 +185,7 @@ export class BoneKind extends Bone {
 
   get boneColor(): Color {
     switch (this.boneT.bodyPart) {
+      case BodyPart.PLAYSPACE:
       case BodyPart.NONE:
         throw 'Unexpected body part';
       case BodyPart.HEAD:
@@ -230,6 +231,7 @@ export class BoneKind extends Bone {
 
   static children(part: BodyPart): BodyPart[] {
     switch (part) {
+      case BodyPart.PLAYSPACE:
       case BodyPart.NONE:
         throw 'Unexpected body part';
       case BodyPart.HEAD:
@@ -283,6 +285,7 @@ export class BoneKind extends Bone {
 
   static parent(part: BodyPart): BodyPart | null {
     switch (part) {
+      case BodyPart.PLAYSPACE:
       case BodyPart.NONE:
         throw 'Unexpected body part';
       case BodyPart.HEAD:

server/core/src/main/java/dev/slimevr/VRServer.kt

@@ -308,12 +308,26 @@ class VRServer @JvmOverloads constructor(
 		queueTask { humanPoseManager.clearTrackersMounting(resetSourceName) }
 	}
 
+	fun getPauseTracking(): Boolean = humanPoseManager.getPauseTracking()
+
 	fun setPauseTracking(pauseTracking: Boolean, sourceName: String?) {
-		queueTask { humanPoseManager.setPauseTracking(pauseTracking, sourceName) }
+		queueTask {
+			humanPoseManager.setPauseTracking(pauseTracking, sourceName)
+			// Toggle trackers as they don't toggle when tracking is paused
+			if (this.getVRBridge(ISteamVRBridge::class.java)?.updateShareSettingsAutomatically() == true) {
+				RPCSettingsHandler.sendSteamVRUpdatedSettings(protocolAPI, protocolAPI.rpcHandler)
+			}
+		}
 	}
 
 	fun togglePauseTracking(sourceName: String?) {
-		queueTask { humanPoseManager.togglePauseTracking(sourceName) }
+		queueTask {
+			humanPoseManager.togglePauseTracking(sourceName)
+			// Toggle trackers as they don't toggle when tracking is paused
+			if (this.getVRBridge(ISteamVRBridge::class.java)?.updateShareSettingsAutomatically() == true) {
+				RPCSettingsHandler.sendSteamVRUpdatedSettings(protocolAPI, protocolAPI.rpcHandler)
+			}
+		}
 	}
 
 	fun scheduleResetTrackersFull(resetSourceName: String?, delay: Long) {

server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt

@@ -461,7 +461,7 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
 		val req = messageHeader
 			.message(SetPauseTrackingRequest()) as? SetPauseTrackingRequest ?: return
 
-		api.server.humanPoseManager.setPauseTracking(req.pauseTracking(), RESET_SOURCE_NAME)
+		api.server.setPauseTracking(req.pauseTracking(), RESET_SOURCE_NAME)
 	}
 
 	fun onHeightRequest(conn: GenericConnection, messageHeader: RpcMessageHeader?) {

server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt

@@ -69,12 +69,19 @@ class Bone(val boneType: BoneType) {
 	fun getLocalRotation(): Quaternion = headNode.localTransform.rotation
 
 	/**
-	 * Sets the global rotation of the bone
+	 * Sets the global rotation of the bone with the rotationOffset
 	 */
 	fun setRotation(rotation: Quaternion) {
 		headNode.localTransform.rotation = rotation * rotationOffset
 	}
 
+	/**
+	 * Sets the global rotation of the bone
+	 */
+	fun setRawRotation(rotation: Quaternion) {
+		headNode.localTransform.rotation = rotation
+	}
+
 	/**
 	 * Returns the global position of the head of the bone
 	 */

server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt

@@ -342,7 +342,7 @@ class HumanPoseManager(val server: VRServer?) {
 	 */
 	val trackersToReset: List<Tracker?>
 		get() =
-			server?.allTrackers ?: skeleton.localTrackers
+			server?.allTrackers ?: skeleton.trackersToReset
 
 	/**
 	 * @return the head bone, which is the root of the skeleton
@@ -690,6 +690,8 @@ class HumanPoseManager(val server: VRServer?) {
 		get() = skeletonConfigManager.userHeightFromOffsets
 
 	// #endregion
+	fun getPauseTracking(): Boolean = skeleton.getPauseTracking()
+
 	fun setPauseTracking(pauseTracking: Boolean, sourceName: String?) {
 		skeleton.setPauseTracking(pauseTracking, sourceName)
 	}

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt

@@ -103,6 +103,7 @@ class HumanSkeleton(
 	var rightHandTracker: Tracker? = null
 	var leftShoulderTracker: Tracker? = null
 	var rightShoulderTracker: Tracker? = null
+	var playspaceTracker: Tracker? = null
 
 	// Output trackers
 	var computedHeadTracker: Tracker? = null
@@ -278,6 +279,7 @@ class HumanSkeleton(
 		rightHandTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_HAND)
 		leftShoulderTracker = getTrackerForSkeleton(trackers, TrackerPosition.LEFT_SHOULDER)
 		rightShoulderTracker = getTrackerForSkeleton(trackers, TrackerPosition.RIGHT_SHOULDER)
+		playspaceTracker = getTrackerForSkeleton(trackers, TrackerPosition.PLAYSPACE)
 
 		// Check for specific conditions and store them in booleans.
 		hasSpineTracker = upperChestTracker != null || chestTracker != null || waistTracker != null || hipTracker != null
@@ -354,18 +356,45 @@ class HumanSkeleton(
 	 */
 	@VRServerThread
 	fun updatePose() {
+		// Check taps for resets
 		tapDetectionManager.update()
 
+		// Do FK
 		updateTransforms()
+
+		// Update bones and output trackers
 		updateBones()
 		updateComputedTrackers()
 
 		// Don't run post-processing if the tracking is paused
 		if (pauseTracking) return
 
+		// Run Legtweaks
 		legTweaks.tweakLegs()
+
+		// Run Mocap mode
 		localizer.update()
+
+		// Run Vive hip emulation
 		viveEmulation.update()
+
+		// Playspace motion compensation
+		playspaceTracker?.let {
+			if (it.hasRotation) {
+				val motionCompensationRotOffset = it.getRotation().inv()
+				for (bone in allBones) {
+					bone.setRawRotation(motionCompensationRotOffset * bone.getLocalRotation())
+				}
+			}
+			if (it.hasPosition) {
+				// TODO motion compensate for position as well?
+				// headBone.setPosition(headBone.getPosition() - it.position)
+			}
+
+			// Update bones and output trackers
+			updateBones()
+			updateComputedTrackers()
+		}
 	}
 
 	/**
@@ -390,6 +419,7 @@ class HumanSkeleton(
 
 		// Spine
 		updateSpineTransforms()
+
 		// Left leg
 		updateLegTransforms(
 			leftUpperLegBone,
@@ -401,6 +431,7 @@ class HumanSkeleton(
 			leftLowerLegTracker,
 			leftFootTracker,
 		)
+
 		// Right leg
 		updateLegTransforms(
 			rightUpperLegBone,
@@ -412,6 +443,7 @@ class HumanSkeleton(
 			rightLowerLegTracker,
 			rightFootTracker,
 		)
+
 		// Left arm
 		updateArmTransforms(
 			isTrackingLeftArmFromController,
@@ -426,6 +458,7 @@ class HumanSkeleton(
 			leftLowerArmTracker,
 			leftHandTracker,
 		)
+
 		// Right arm
 		updateArmTransforms(
 			isTrackingRightArmFromController,
@@ -982,6 +1015,34 @@ class HumanSkeleton(
 			rightHandBone,
 		)
 
+	/**
+	 * Returns an array of all the bones, trackers or not
+	 */
+	private val allBones: Array<Bone>
+		get() = arrayOf(
+			headBone,
+			neckBone,
+			headTrackerBone,
+			upperChestBone,
+			chestBone,
+			chestTrackerBone,
+			waistBone,
+			hipBone,
+			hipTrackerBone,
+			leftHipBone,
+			rightHipBone,
+			leftUpperLegBone,
+			leftKneeTrackerBone,
+			rightUpperLegBone,
+			rightKneeTrackerBone,
+			leftLowerLegBone,
+			rightLowerLegBone,
+			leftFootBone,
+			leftFootTrackerBone,
+			rightFootBone,
+			rightFootTrackerBone,
+		) + allArmBones
+
 	/**
 	 * Returns all the arm bones, tracker or not.
 	 */
@@ -1023,7 +1084,7 @@ class HumanSkeleton(
 	 */
 	val isTrackingRightArmFromController: Boolean
 		get() = rightHandTracker != null && rightHandTracker!!.hasPosition && !forceArmsFromHMD
-	val localTrackers: List<Tracker?>
+	val trackersToReset: List<Tracker?>
 		get() = listOf(
 			neckTracker,
 			chestTracker,
@@ -1046,10 +1107,8 @@ class HumanSkeleton(
 		)
 
 	fun resetTrackersFull(resetSourceName: String?) {
-		val trackersToReset = humanPoseManager.trackersToReset
-
-		// Resets all axis of the trackers with the HMD as reference.
 		var referenceRotation = IDENTITY
+		// Reset the head tracker from identity if needed, else store its rotation
 		headTracker?.let {
 			if (it.needsReset) {
 				it.resetsHandler.resetFull(referenceRotation)
@@ -1057,11 +1116,14 @@ class HumanSkeleton(
 				referenceRotation = it.getRotation()
 			}
 		}
+		// Resets the trackers with the HMD as reference.
 		for (tracker in trackersToReset) {
 			if (tracker != null && tracker.needsReset) {
 				tracker.resetsHandler.resetFull(referenceRotation)
 			}
 		}
+		// Reset the playspace tracker from identity rotation
+		playspaceTracker?.resetsHandler?.resetFull(IDENTITY)
 
 		// Tell floorclip to reset its floor level on the next update
 		// of the computed trackers
@@ -1075,10 +1137,8 @@ class HumanSkeleton(
 
 	@VRServerThread
 	fun resetTrackersYaw(resetSourceName: String?) {
-		val trackersToReset = humanPoseManager.trackersToReset
-
-		// Resets the yaw of the trackers with the head as reference.
 		var referenceRotation = IDENTITY
+		// Reset the head tracker from identity if needed, else store its rotation
 		headTracker?.let {
 			if (it.needsReset) {
 				it.resetsHandler.resetYaw(referenceRotation)
@@ -1086,22 +1146,23 @@ class HumanSkeleton(
 				referenceRotation = it.getRotation()
 			}
 		}
+		// Resets the trackers with the HMD as reference.
 		for (tracker in trackersToReset) {
 			if (tracker != null && tracker.needsReset) {
 				tracker.resetsHandler.resetYaw(referenceRotation)
 			}
 		}
+		// Reset the playspace tracker from identity rotation
+		playspaceTracker?.resetsHandler?.resetYaw(IDENTITY)
+
 		legTweaks.resetBuffer()
 		LogManager.info(String.format("[HumanSkeleton] Reset: yaw (%s)", resetSourceName))
 	}
 
 	@VRServerThread
 	fun resetTrackersMounting(resetSourceName: String?) {
-		val trackersToReset = humanPoseManager.trackersToReset
-
-		// Resets the mounting orientation of the trackers with the HMD as
-		// reference.
 		var referenceRotation = IDENTITY
+		// Reset the head tracker from identity if needed, else store its rotation
 		headTracker?.let {
 			if (it.needsMounting) {
 				it.resetsHandler.resetMounting(referenceRotation)
@@ -1109,11 +1170,13 @@ class HumanSkeleton(
 				referenceRotation = it.getRotation()
 			}
 		}
+		// Resets the trackers with the HMD as reference.
 		for (tracker in trackersToReset) {
 			if (tracker != null && tracker.needsMounting) {
 				tracker.resetsHandler.resetMounting(referenceRotation)
 			}
 		}
+
 		legTweaks.resetBuffer()
 		localizer.reset()
 		LogManager.info(String.format("[HumanSkeleton] Reset: mounting (%s)", resetSourceName))
@@ -1121,7 +1184,6 @@ class HumanSkeleton(
 
 	@VRServerThread
 	fun clearTrackersMounting(resetSourceName: String?) {
-		val trackersToReset = humanPoseManager.trackersToReset
 		headTracker?.let {
 			if (it.needsMounting) it.resetsHandler.clearMounting()
 		}

server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerPosition.kt

@@ -36,6 +36,7 @@ enum class TrackerPosition(
 	RIGHT_HAND("body:right_hand", TrackerRole.RIGHT_HAND, BodyPart.RIGHT_HAND),
 	LEFT_SHOULDER("body:left_shoulder", TrackerRole.LEFT_SHOULDER, BodyPart.LEFT_SHOULDER),
 	RIGHT_SHOULDER("body:right_shoulder", TrackerRole.RIGHT_SHOULDER, BodyPart.RIGHT_SHOULDER),
+	PLAYSPACE("body:playspace", null, BodyPart.PLAYSPACE),
 	;
 
 	/**

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/FeatureFlags.kt

@@ -44,6 +44,11 @@ enum class ServerFeatureFlags {
 	/** Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). */
 	PROTOCOL_BUNDLE_SUPPORT,
 
+	/** Server can parse bundle packets with compact headers and packed IMU rotation/acceleration frames:
+	 - `PACKET_BUNDLE_COMPACT` = 101 (0x65),
+	 - `PACKET_ROTATION_AND_ACCELERATION` = 23 (0x17). */
+	PROTOCOL_BUNDLE_COMPACT_SUPPORT,
+
 	// Add new flags here
 
 	BITS_TOTAL, ;
@@ -51,6 +56,7 @@ enum class ServerFeatureFlags {
 	companion object {
 		val flagsEnabled: Set<ServerFeatureFlags> = setOf(
 			PROTOCOL_BUNDLE_SUPPORT,
+			PROTOCOL_BUNDLE_COMPACT_SUPPORT,
 
 			// Add enabled flags here
 		)

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt

@@ -322,7 +322,8 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 				if (tracker == null) return
 				tracker.setRotation(rot)
 				if (packet is UDPPacket23RotationAndAcceleration) {
-					tracker.setAcceleration(packet.acceleration)
+					// Switch x and y around to adjust for different axes
+					tracker.setAcceleration(Vector3(packet.acceleration.y, packet.acceleration.x, packet.acceleration.z))
 				}
 				tracker.dataTick()
 			}

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt

@@ -40,6 +40,25 @@ class UDPProtocolParser {
 				buf.position(bundlePacketStart + bundlePacketLen)
 			}
 			return bundlePackets.toTypedArray()
+		} else if (packetId == PACKET_BUNDLE_COMPACT) {
+			bundlePackets.clear()
+			while (buf.hasRemaining()) {
+				val bundlePacketLen = Math.min(buf.get().toUByte().toInt(), buf.remaining()) // 1 byte
+				if (bundlePacketLen == 0) continue
+
+				val bundlePacketStart = buf.position()
+				val bundleBuf = buf.slice()
+				bundleBuf.limit(bundlePacketLen)
+				val bundlePacketId = bundleBuf.get().toUByte().toInt() // 1 byte
+				val newPacket = getNewPacket(bundlePacketId)
+				newPacket?.let {
+					newPacket.readData(bundleBuf)
+					bundlePackets.add(newPacket)
+				}
+
+				buf.position(bundlePacketStart + bundlePacketLen)
+			}
+			return bundlePackets.toTypedArray()
 		}
 
 		val newPacket = getNewPacket(packetId)
@@ -129,6 +148,7 @@ class UDPProtocolParser {
 		const val PACKET_FEATURE_FLAGS = 22
 		const val PACKET_ROTATION_AND_ACCELERATION = 23
 		const val PACKET_BUNDLE = 100
+		const val PACKET_BUNDLE_COMPACT = 101
 		const val PACKET_PROTOCOL_CHANGE = 200
 		private val HANDSHAKE_BUFFER = ByteArray(64)
 		private val bundlePackets = ArrayList<UDPPacket>(128)

server/desktop/src/main/java/dev/slimevr/desktop/platform/SteamVRBridge.kt

@@ -20,7 +20,7 @@ import solarxr_protocol.rpc.StatusDataUnion
 import solarxr_protocol.rpc.StatusSteamVRDisconnectedT
 
 abstract class SteamVRBridge(
-	server: VRServer,
+	protected val server: VRServer,
 	threadName: String,
 	bridgeName: String,
 	protected val bridgeSettingsKey: String,
@@ -53,7 +53,9 @@ abstract class SteamVRBridge(
 	}
 
 	override fun updateShareSettingsAutomatically(): Boolean {
-		if (!config.automaticSharedTrackersToggling) return false
+		// Return false if automatic trackers is disabled or if tracking is paused
+		if (!config.automaticSharedTrackersToggling || server.getPauseTracking()) return false
+
 		val skeleton = instance.humanPoseManager.skeleton
 		val isWaistSteamVr = skeleton.hipTracker?.device?.isOpenVrDevice == true ||
 			skeleton.waistTracker?.device?.isOpenVrDevice == true
@@ -110,6 +112,7 @@ abstract class SteamVRBridge(
 		changeShareSettings(TrackerRole.RIGHT_ELBOW, hasRightElbow && !isRightElbowSteamVr)
 
 		// Hands aren't touched as they will override the controller's tracking
+		// Return true to say that trackers were successfully toggled automatically
 		return true
 	}
 

server/desktop/src/main/java/dev/slimevr/desktop/tracking/trackers/hid/TrackersHID.kt

@@ -16,9 +16,14 @@ import org.hid4java.HidServices
 import org.hid4java.HidServicesListener
 import org.hid4java.HidServicesSpecification
 import org.hid4java.event.HidServicesEvent
+import org.hid4java.jna.HidApi
+import org.hid4java.jna.HidDeviceInfoStructure
 import java.util.function.Consumer
 import kotlin.experimental.and
 
+private const val HID_TRACKER_RECEIVER_VID = 0x2FE3
+private const val HID_TRACKER_RECEIVER_PID = 0x5652
+
 /**
  * Receives trackers data by UDP using extended owoTrack protocol.
  */
@@ -39,10 +44,16 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 		dataReadThread.isDaemon = true
 		dataReadThread.name = "hid4java data reader"
 		dataReadThread.start()
+		// We use hid4java but actually do not start the service ever, because it will just enumerate everything and cause problems
+		// Do enumeration ourself
+		val deviceEnumerateThread = Thread(deviceEnumerateRunnable)
+		deviceEnumerateThread.isDaemon = true
+		deviceEnumerateThread.name = "hid4java device enumerator"
+		deviceEnumerateThread.start()
 	}
 
 	private fun checkConfigureDevice(hidDevice: HidDevice) {
-		if (hidDevice.vendorId == 0x2FE3 && hidDevice.productId == 0x5652) { // TODO: Use correct ids
+		if (hidDevice.vendorId == HID_TRACKER_RECEIVER_VID && hidDevice.productId == HID_TRACKER_RECEIVER_PID) { // TODO: Use correct ids
 			if (hidDevice.isClosed) {
 				check(hidDevice.open()) { "Unable to open device" }
 			}
@@ -141,13 +152,6 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 	@get:Synchronized
 	private val dataReadRunnable: Runnable
 		get() = Runnable {
-			try {
-				sleep(100) // Delayed start
-			} catch (e: InterruptedException) {
-				currentThread().interrupt()
-				return@Runnable
-			}
-			hidServices.start()
 			while (true) {
 				try {
 					sleep(0) // Possible performance impact
@@ -159,9 +163,31 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 			}
 		}
 
+	@get:Synchronized
+	private val deviceEnumerateRunnable: Runnable
+		get() = Runnable {
+			try {
+				sleep(100) // Delayed start
+			} catch (e: InterruptedException) {
+				currentThread().interrupt()
+				return@Runnable
+			}
+			while (true) {
+				try {
+					sleep(1000)
+				} catch (e: InterruptedException) {
+					currentThread().interrupt()
+					break
+				}
+				deviceEnumerate() // not in try catch?
+			}
+		}
+
 	private fun dataRead() {
 		synchronized(devicesByHID) {
 			var devicesPresent = false
+			val q = intArrayOf(0, 0, 0, 0)
+			val a = intArrayOf(0, 0, 0)
 			for ((hidDevice, deviceList) in devicesByHID) {
 				val dataReceived: Array<Byte> = try {
 					hidDevice.read(80, 0) // Read up to 80 bytes
@@ -183,20 +209,17 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 						val idCombination = dataReceived[i + 1].toInt()
 						val rssi = -dataReceived[i + 2].toInt()
 						val battery = dataReceived[i + 3].toInt()
-						val battery_mV = dataReceived[i + 5].toInt() and 255 shl 8 or (dataReceived[i + 4].toInt() and 255)
-						val q = floatArrayOf(0f, 0f, 0f, 0f)
-						val a = floatArrayOf(0f, 0f, 0f)
-						for (j in 0..3) { // quat received as fixed 14
-							var buf =
-								dataReceived[i + 6 + j * 2 + 1].toInt() and 255 shl 8 or (dataReceived[i + 6 + j * 2].toInt() and 255)
-							buf -= 32768 // uint to int
-							q[j] = buf / (1 shl 14).toFloat() // fixed 14 to float
+						// ushort big endian
+						val battery_mV = dataReceived[i + 5].toUByte().toInt() shl 8 or dataReceived[i + 4].toUByte().toInt()
+						// Q15: 1 is represented as 0x7FFF, -1 as 0x8000
+						// The sender can use integer saturation to avoid overflow
+						for (j in 0..3) { // quat received as fixed Q15
+							// Q15 as short big endian
+							q[j] = dataReceived[i + 6 + j * 2 + 1].toInt() shl 8 or dataReceived[i + 6 + j * 2].toUByte().toInt()
 						}
 						for (j in 0..2) { // accel received as fixed 7, in m/s
-							var buf =
-								dataReceived[i + 14 + j * 2 + 1].toInt() and 255 shl 8 or (dataReceived[i + 14 + j * 2].toInt() and 255)
-							buf -= 32768 // uint to int
-							a[j] = buf / (1 shl 7).toFloat() // fixed 7 to float
+							// Q15 as short big endian
+							a[j] = dataReceived[i + 14 + j * 2 + 1].toInt() shl 8 or dataReceived[i + 14 + j * 2].toUByte().toInt()
 						}
 						val trackerId = idCombination and 0b1111
 						val deviceId = (idCombination shr 4) and 0b1111
@@ -209,11 +232,17 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 						// tracker.batteryVoltage = if (battery and 128 == 128) 5f else 0f // Charge status
 						tracker.batteryVoltage = battery_mV.toFloat() * 0.001f
 						tracker.batteryLevel = (battery and 127).toFloat()
-						var rot = Quaternion(q[0], q[1], q[2], q[3])
-						rot = AXES_OFFSET.times(rot)
+						// The data comes in the same order as in the UDP protocol
+						// x y z w -> w x y z
+						var rot = Quaternion(q[3].toFloat(), q[0].toFloat(), q[1].toFloat(), q[2].toFloat())
+						val scaleRot = 1 / (1 shl 15).toFloat() // compile time evaluation
+						rot = AXES_OFFSET.times(scaleRot).times(rot) // no division
 						tracker.setRotation(rot)
 						// TODO: I think the acceleration is wrong???
-						var acceleration = Vector3(a[0], a[1], a[2])
+						// Yes it was. And rotation was wrong too.
+						// At lease we have fixed the fixed point decoding.
+						val scaleAccel = 1 / (1 shl 7).toFloat() // compile time evaluation
+						var acceleration = Vector3(a[0].toFloat(), a[1].toFloat(), a[2].toFloat()).times(scaleAccel) // no division
 						tracker.setAcceleration(acceleration)
 						tracker.dataTick()
 						i += 20
@@ -227,25 +256,52 @@ class TrackersHID(name: String, private val trackersConsumer: Consumer<Tracker>)
 		}
 	}
 
+	private fun deviceEnumerate() {
+		var root: HidDeviceInfoStructure? = null
+		try {
+			root = HidApi.enumerateDevices(HID_TRACKER_RECEIVER_VID, HID_TRACKER_RECEIVER_PID) // TODO: change to proper vendorId and productId, need to enum all appropriate productId
+		} catch (e: Throwable) {
+			LogManager.severe("[TrackerServer] Couldn't enumerate HID devices", e)
+		}
+		val hidDeviceList: MutableList<HidDevice> = mutableListOf()
+		if (root != null) {
+			var hidDeviceInfoStructure: HidDeviceInfoStructure? = root
+			do {
+				hidDeviceList.add(HidDevice(hidDeviceInfoStructure, null, hidServicesSpecification))
+				hidDeviceInfoStructure = hidDeviceInfoStructure?.next()
+			} while (hidDeviceInfoStructure != null)
+			HidApi.freeEnumeration(root)
+		}
+		synchronized(devicesByHID) {
+			// Work on devicesByHid and add/remove as necessary
+			val removeList: MutableList<HidDevice> = devicesByHID.keys.toMutableList()
+			removeList.removeAll(hidDeviceList)
+			hidDeviceList.removeAll(devicesByHID.keys) // addList
+			for (device in removeList) {
+				removeDevice(device)
+			}
+			for (device in hidDeviceList) {
+				checkConfigureDevice(device)
+			}
+		}
+	}
+
 	override fun run() { // Doesn't seem to run
 	}
 
 	fun getDevices(): List<Device> = devices
 
+	// We don't use these
 	override fun hidDeviceAttached(event: HidServicesEvent) {
-		checkConfigureDevice(event.hidDevice)
 	}
 
 	override fun hidDeviceDetached(event: HidServicesEvent) {
-		removeDevice(event.hidDevice)
 	}
 
 	override fun hidFailure(event: HidServicesEvent) {
-		// TODO:
 	}
 
 	override fun hidDataReceived(p0: HidServicesEvent?) {
-		// Seems to have issues with the size of data returned
 	}
 
 	companion object {