From ae4391bff1ba50205a3fb2a95b3627a1ee56bae6 Mon Sep 17 00:00:00 2001 From: Ilia Ki Date: Mon, 22 Apr 2024 00:22:38 +0700 Subject: [PATCH] Add compact IMU frame (rot+acc) (#1001) --- .../trackers/udp/TrackersUDPServer.kt | 3 +++ .../tracking/trackers/udp/UDPPacket.kt | 21 +++++++++++++++++++ .../trackers/udp/UDPProtocolParser.kt | 1 + 3 files changed, 25 insertions(+) diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt index 6119d976a6..f787b43056 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt @@ -321,6 +321,9 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker tracker = connection?.getTracker(packet.sensorId) if (tracker == null) return tracker.setRotation(rot) + if (packet is UDPPacket23RotationAndAcceleration) { + tracker.setAcceleration(packet.acceleration) + } tracker.dataTick() } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt index bd28192567..6691602acd 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPPacket.kt @@ -329,6 +329,27 @@ class UDPPacket22FeatureFlags( } } +data class UDPPacket23RotationAndAcceleration( + override var rotation: Quaternion = Quaternion.IDENTITY, + var acceleration: Vector3 = Vector3.NULL, +) : UDPPacket(23), + RotationPacket { + override var sensorId: Int = 0 + override fun readData(buf: ByteBuffer) { + // s16 s16 s16 s16 s16 s16 s16 + // qX qY qZ qW aX aY aZ + sensorId = buf.get().toInt() and 0xFF + val scaleR = 1 / (1 shl 15).toFloat() // Q15: 1 is represented as 0x7FFF and -1 as 0x8000 + val x = buf.short * scaleR + val y = buf.short * scaleR + val z = buf.short * scaleR + val w = buf.short * scaleR + rotation = Quaternion(w, x, y, z).unit() + val scaleA = 1 / (1 shl 7).toFloat() // The same as the HID scale + acceleration = Vector3(buf.short * scaleA, buf.short * scaleA, buf.short * scaleA) + } +} + data class UDPPacket200ProtocolChange( var targetProtocol: Int = 0, var targetProtocolVersion: Int = 0, diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt index f1b4276cde..a7476a8a93 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPProtocolParser.kt @@ -127,6 +127,7 @@ class UDPProtocolParser { const val PACKET_TEMPERATURE = 20 const val PACKET_USER_ACTION = 21 const val PACKET_FEATURE_FLAGS = 22 + const val PACKET_ROTATION_AND_ACCELERATION = 23 const val PACKET_BUNDLE = 100 const val PACKET_PROTOCOL_CHANGE = 200 private val HANDSHAKE_BUFFER = ByteArray(64)