More udp tracker data

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

@@ -19,7 +19,9 @@ class Context<S, in A>(
 	val state: StateFlow<S> = mutableStateFlow.asStateFlow()
 
 	fun dispatch(action: A) {
-		mutableStateFlow.update { applyAction(it, action) }
+		mutableStateFlow.update {
+			applyAction(it, action)
+		}
 	}
 
 	fun dispatchAll(actions: List<A>) {

server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt

@@ -21,6 +21,7 @@ import solarxr_protocol.data_feed.tracker.TrackerDataMask
 import solarxr_protocol.data_feed.tracker.TrackerInfo
 import solarxr_protocol.datatypes.DeviceId
 import solarxr_protocol.datatypes.Ipv4Address
+import solarxr_protocol.datatypes.Temperature
 import solarxr_protocol.datatypes.TrackerId
 import solarxr_protocol.datatypes.hardware_info.HardwareInfo
 import solarxr_protocol.datatypes.hardware_info.HardwareStatus
@@ -28,25 +29,28 @@ import solarxr_protocol.datatypes.math.Quat
 import solarxr_protocol.datatypes.math.Vec3f
 import java.nio.ByteBuffer
 
-private fun createTracker(device: DeviceState, tracker: TrackerState, trackerMask: TrackerDataMask): TrackerData = TrackerData(
-	trackerId = TrackerId(
-		trackerNum = tracker.id.toUByte(),
-		deviceId = DeviceId(device.id.toUByte()),
-	),
-	status = if (trackerMask.status == true) device.status else null,
-	rotation = if (trackerMask.rotation == true) tracker.rawRotation.let { Quat(it.x, it.y, it.z, it.w) } else null,
-	position = if (trackerMask.position == true && tracker.position != null) tracker.position.let { Vec3f(it.x, it.y, it.z) } else null,
-	info = if (trackerMask.info == true) {
-		TrackerInfo(
-			imuType = tracker.sensorType,
-			bodyPart = tracker.bodyPart,
-			displayName = tracker.name,
-		)
-	} else {
-		null
-	},
-	tps = if (trackerMask.tps == true) tracker.tps else null,
-)
+private fun createTracker(device: DeviceState, tracker: TrackerState, trackerMask: TrackerDataMask, datafeedConfig: DataFeedConfig): TrackerData {
+	return TrackerData(
+		trackerId = TrackerId(
+			trackerNum = tracker.id.toUByte(),
+			deviceId = DeviceId(device.id.toUByte()),
+		),
+		status = if (trackerMask.status == true) device.status else null,
+		rotation = if (trackerMask.rotation == true) tracker.rawRotation.let { Quat(it.x, it.y, it.z, it.w) } else null,
+		position = if (trackerMask.position == true && tracker.position != null) tracker.position.let { Vec3f(it.x, it.y, it.z) } else null,
+		info = if (trackerMask.info == true) {
+			TrackerInfo(
+				imuType = tracker.sensorType,
+				bodyPart = tracker.bodyPart,
+				displayName = tracker.name,
+			)
+		} else {
+			null
+		},
+		tps = if (trackerMask.tps == true) tracker.tps else null,
+		temp = if (trackerMask.temp == true && tracker.imuTemp != null) Temperature(temp = tracker.imuTemp) else null,
+	)
+}
 
 private fun createDevice(
 	device: DeviceState,
@@ -73,7 +77,7 @@ private fun createDevice(
 		),
 		trackers = if (trackerMask != null) {
 			trackers.filter { it.deviceId == device.id }
-				.map { tracker -> createTracker(device, tracker, trackerMask) }
+				.map { tracker -> createTracker(device, tracker, trackerMask, datafeedConfig) }
 		} else {
 			null
 		},

server/core/src/main/java/dev/slimevr/tracker/module.kt

@@ -22,6 +22,7 @@ data class TrackerState(
 	val deviceId: Int,
 	val origin: DeviceOrigin,
 	val tps: UShort,
+	val imuTemp: Float?,
 	val position: Vector3?,
 )
 
@@ -56,6 +57,7 @@ class Tracker(
 				sensorType = sensorType,
 				position = null,
 				tps = 0u,
+				imuTemp = null,
 			)
 
 			val behaviours = listOf(TrackerBasicBehaviour, TrackerTPSBehaviour)

server/core/src/main/java/dev/slimevr/udp/behaviours.kt

@@ -226,6 +226,16 @@ object SensorRotationBehaviour : UDPConnectionBehaviour {
 			val tracker = receiver.getTracker(event.data.sensorId) ?: return@onPacket
 			tracker.context.dispatch(TrackerActions.Update { copy(rawRotation = event.data.rotation) })
 		}
+
+		receiver.packetEvents.onPacket<RotationAndAccel> { event ->
+			val tracker = receiver.getTracker(event.data.sensorId) ?: return@onPacket
+			tracker.context.dispatch(TrackerActions.Update { copy(rawRotation = event.data.rotation) })
+		}
+
+		receiver.packetEvents.onPacket<Rotation2> { event ->
+			val tracker = receiver.getTracker(event.data.sensorId) ?: return@onPacket
+			tracker.context.dispatch(TrackerActions.Update { copy(rawRotation = event.data.rotation) })
+		}
 	}
 }
 
@@ -270,3 +280,12 @@ object FlagsBehaviour : UDPConnectionBehaviour {
 		}
 	}
 }
+
+object TemperatureBehaviour : UDPConnectionBehaviour {
+	override fun observe(receiver: UDPConnection) {
+		receiver.packetEvents.onPacket<Temperature> { event ->
+			val tracker = receiver.getTracker(event.data.sensorId) ?: return@onPacket
+			tracker.context.dispatch(TrackerActions.Update { copy(imuTemp = event.data.temp) })
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/udp/connection.kt

@@ -94,7 +94,8 @@ class UDPConnection(
 				SensorInfoBehaviour,
 				SensorRotationBehaviour,
 				BundledPacketBehaviour,
-				FlagsBehaviour
+				FlagsBehaviour,
+				TemperatureBehaviour
 			)
 
 			val context = Context.create(

server/core/src/main/java/dev/slimevr/udp/packets.kt

@@ -496,6 +496,7 @@ fun writePacket(dst: Sink, packet: UDPPacket) {
 
 data class PacketEvent<out T : UDPPacket>(
 	val data: T,
+	// Packet number is optional for the inner packets of a bundle packet
 	val packetNumber: Long?,
 )
 

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

@@ -34,6 +34,7 @@ suspend fun createUDPTrackerServer(
 	val recvPacket = DatagramPacket(recvBuffer, recvBuffer.size)
 
 	supervisorScope {
+		val udpScope = this
 		launch(Dispatchers.IO) {
 			while (isActive) {
 				socket.receive(recvPacket)
@@ -64,7 +65,7 @@ suspend fun createUDPTrackerServer(
 							remotePort = port,
 							socket = socket,
 							serverContext = serverContext,
-							scope = this,
+							scope = udpScope,
 						)
 
 						state.connections[ip] = newContext

server/core/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt

@@ -8,9 +8,8 @@ import kotlin.math.sin
 
 enum class EulerOrder { XYZ, YZX, ZXY, ZYX, YXZ, XZY }
 
-@JvmInline
 @Serializable
-value class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z: Float) {
+class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z: Float) {
 	operator fun component1(): EulerOrder = order
 	operator fun component2(): Float = x
 	operator fun component3(): Float = y

server/core/src/main/java/io/github/axisangles/ktmath/Matrix3.kt

@@ -5,9 +5,8 @@ package io.github.axisangles.ktmath
 import kotlinx.serialization.Serializable
 import kotlin.math.*
 
-@JvmInline
 @Serializable
-value class Matrix3
+class Matrix3
 @Suppress("ktlint") constructor(
 	val xx: Float, val yx: Float, val zx: Float,
 	val xy: Float, val yy: Float, val zy: Float,
@@ -474,4 +473,4 @@ data class ObjectMatrix3(
 
 operator fun Float.times(that: Matrix3): Matrix3 = that * this
 
-operator fun Float.div(that: Matrix3): Matrix3 = that.inv() * this
+operator fun Float.div(that: Matrix3): Matrix3 = that.inv() * this

server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt

@@ -5,9 +5,8 @@ package io.github.axisangles.ktmath
 import kotlinx.serialization.Serializable
 import kotlin.math.*
 
-@JvmInline
 @Serializable
-value class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
+class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
 	companion object {
 		val NULL = Quaternion(0f, 0f, 0f, 0f)
 		val IDENTITY = Quaternion(1f, 0f, 0f, 0f)

server/core/src/main/java/io/github/axisangles/ktmath/Vector3.kt

@@ -5,9 +5,8 @@ package io.github.axisangles.ktmath
 import kotlinx.serialization.Serializable
 import kotlin.math.*
 
-@JvmInline
 @Serializable
-value class Vector3(val x: Float, val y: Float, val z: Float) {
+class Vector3(val x: Float, val y: Float, val z: Float) {
 	companion object {
 		val NULL = Vector3(0f, 0f, 0f)
 		val POS_X = Vector3(1f, 0f, 0f)