diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt index 4c96e6f06..6392d2f7c 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt @@ -115,6 +115,7 @@ class Tracker @JvmOverloads constructor( var ping: Int? = null var signalStrength: Int? = null var temperature: Float? = null + var button: Int? = null var customName: String? = null var magStatus: MagnetometerStatus = magStatus private set diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt index 79879fa41..23d0a27ba 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt @@ -143,6 +143,7 @@ class HIDCommon { var temp: Int? = null var brd_id: Int? = null var mcu_id: Int? = null + var button: Int? = null // var imu_id: Int? = null // var mag_id: Int? = null var fw_date: Int? = null @@ -220,6 +221,28 @@ class HIDCommon { } } + 6 -> { // data + button = dataReceived[i + 2].toUByte().toInt() + rssi = dataReceived[i + 15].toUByte().toInt() + } + + 7 -> { // reduced precision quat and accel with data + button = dataReceived[i + 2].toUByte().toInt() + // quaternion is quantized as exponential map + // X = 10 bits, Y/Z = 11 bits + val buffer = ByteBuffer.wrap(dataReceived, i + 5, 4) + buffer.order(java.nio.ByteOrder.LITTLE_ENDIAN) + val q_buf = buffer.getInt().toUInt() + q[0] = (q_buf and 1023u).toInt() + q[1] = (q_buf shr 10 and 2047u).toInt() + q[2] = (q_buf shr 21 and 2047u).toInt() + for (j in 0..2) { // accel received as fixed 7, in m/s^2 + // Q7 as short little endian + a[j] = dataReceived[i + 9 + j * 2 + 1].toInt() shl 8 or dataReceived[i + 9 + j * 2].toUByte().toInt() + } + rssi = dataReceived[i + 15].toUByte().toInt() + } + else -> { } } @@ -248,6 +271,15 @@ class HIDCommon { device.mcuType = mcuType!! } } + if (button != null) { + if (tracker.button == null) { + tracker.button = 0 + } + if (button != tracker.button) { + button = button and tracker.button!!.inv() + // Nothing to do now.. + } + } if (fw_date != null && fw_major != null && fw_minor != null && fw_patch != null) { val firmwareYear = 2020 + (fw_date shr 9 and 127) val firmwareMonth = fw_date shr 5 and 15 @@ -274,7 +306,7 @@ class HIDCommon { rot = AXES_OFFSET.times(scaleRot).times(rot) // no division tracker.setRotation(rot) } - if (packetType == 2) { + if (packetType == 2 || packetType == 7) { val v = floatArrayOf(q[0].toFloat(), q[1].toFloat(), q[2].toFloat()) // used q array for quantized data v[0] /= (1 shl 10).toFloat() v[1] /= (1 shl 11).toFloat() @@ -293,7 +325,7 @@ class HIDCommon { rot = AXES_OFFSET.times(rot) // no division tracker.setRotation(rot) } - if (packetType == 1 || packetType == 2) { + if (packetType == 1 || packetType == 2 || packetType == 7) { // Acceleration is in local device frame // On flat surface / face up: // Right side of the device is +X @@ -317,7 +349,7 @@ class HIDCommon { val magnetometer = Vector3(m[0].toFloat(), m[1].toFloat(), m[2].toFloat()).times(scaleMag) // no division tracker.setMagVector(magnetometer) } - if (packetType == 1 || packetType == 2 || packetType == 4) { + if (packetType == 1 || packetType == 2 || packetType == 4 || packetType == 7) { tracker.dataTick() // only data tick if there is rotation data } }