Add more compact bundle protocol

SlimeVR · Apr 23, 2024 · f71969a · f71969a
1 parent fde8ccd
commit f71969a
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 0 deletions.
diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/FeatureFlags.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/FeatureFlags.kt
@@ -43,6 +43,10 @@ class FirmwareFeatures {
 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
 
@@ -51,6 +55,7 @@ enum class ServerFeatureFlags {
 	companion object {
 		val flagsEnabled: Set<ServerFeatureFlags> = setOf(
 			PROTOCOL_BUNDLE_SUPPORT,
+			PROTOCOL_BUNDLE_COMPACT_SUPPORT,
 
 			// Add enabled flags here
 		)

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
@@ -40,6 +40,25 @@ class UDPProtocolParser {
 				buf.position(bundlePacketStart + bundlePacketLen)
 			}
 			return bundlePackets.toTypedArray()
+		} else if (packetId == PACKET_BUNDLE_V2) {
+			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)
@@ -128,6 +147,7 @@ class UDPProtocolParser {
 		const val PACKET_USER_ACTION = 21
 		const val PACKET_FEATURE_FLAGS = 22
 		const val PACKET_BUNDLE = 100
+		const val PACKET_BUNDLE_V2 = 101
 		const val PACKET_PROTOCOL_CHANGE = 200
 		private val HANDSHAKE_BUFFER = ByteArray(64)
 		private val bundlePackets = ArrayList<UDPPacket>(128)