hid packet 6 and packet 7 (data2) (#1679)

Author: Eirenliel

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

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
 			}
 		}