Fix magnetometer status not changing when restarting HID tracker

gui/src/components/widgets/IMUVisualizerWidget.tsx

@@ -130,14 +130,18 @@ async function initializeIMUVisualizer(
       accelArrow.setLength(Math.sqrt(accelLength) * 2);
     }
 
-    const magVec = new Vector3(mag.x, mag.y, mag.z);
+    const magVec = Vector3FromVec3fT(mag);
     const magLen = magVec.length();
     const magMag = Math.sqrt(magLen / 100);
     if (magLen > 0) {
+      if (magArrow.parent === null) scene.add(magArrow);
+
       const magDir = magVec.clone().normalize();
       magArrow.position.copy(magDir.clone().multiplyScalar(-magMag));
       magArrow.setDirection(magDir);
       magArrow.setLength(2 * magMag);
+    } else {
+      magArrow.removeFromParent();
     }
   };
 
@@ -207,9 +211,7 @@ function IMUVisualizerCanvas({
   }, [model]);
 
   useEffect(() => {
-    if (contextRef.current) {
-      contextRef.current.update(quat, vec, mag);
-    }
+    contextRef.current?.update(quat, vec, mag);
   }, [quat, vec, mag]);
 
   if (error) {

server/core/src/main/java/dev/slimevr/config/ServerConfig.kt

@@ -31,7 +31,7 @@ class ServerConfig {
 // 							it.setMag(false)
 // 						} else {
 						trackers.map { (_, t) ->
-							async { it.setMag(false, t.trackerNum) }
+							async { it.setMag(MagnetometerStatus.DISABLED, t.trackerNum) }
 						}.awaitAll()
 // 						}
 						return@async
@@ -51,7 +51,7 @@ class ServerConfig {
 						.map { (_, t) ->
 							async {
 								// FIXME: Tracker gets restarted after each setMag, what will happen for devices with 3 trackers?
-								it.setMag(true, t.trackerNum)
+								it.setMag(MagnetometerStatus.ENABLED, t.trackerNum)
 							}
 						}.awaitAll()
 				}

server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt

@@ -28,6 +28,7 @@ import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByBodyPart
 import dev.slimevr.tracking.trackers.TrackerStatus
 import dev.slimevr.tracking.trackers.TrackerUtils.getTrackerForSkeleton
+import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
 import io.eiren.util.logging.LogManager
 import io.github.axisangles.ktmath.Quaternion
 import kotlinx.coroutines.*
@@ -517,7 +518,7 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
 
 		mainScope.launch {
 			withTimeoutOrNull(MAG_TIMEOUT) {
-				tracker.device.setMag(state, tracker.trackerNum)
+				tracker.device.setMag(if (state) MagnetometerStatus.ENABLED else MagnetometerStatus.DISABLED, tracker.trackerNum)
 			}
 
 			val fbb = FlatBufferBuilder(32)

server/core/src/main/java/dev/slimevr/tracking/trackers/Device.kt

@@ -2,6 +2,7 @@ package dev.slimevr.tracking.trackers
 
 import dev.slimevr.tracking.trackers.udp.BoardType
 import dev.slimevr.tracking.trackers.udp.MCUType
+import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
 import kotlinx.coroutines.CoroutineScope
 import kotlinx.coroutines.Job
 import kotlinx.coroutines.delay
@@ -41,7 +42,7 @@ open class Device(val magSupport: Boolean = false) {
 	 * if `sensorId` null or in the specified tracker
 	 * @param sensorId If null, every sensor will be modified
 	 */
-	open suspend fun setMag(state: Boolean, sensorId: Int = 255) {
+	open suspend fun setMag(status: MagnetometerStatus, sensorId: Int = 255) {
 		TODO("Not implemented because no mag support: $magSupport")
 	}
 

server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt

@@ -148,8 +148,12 @@ class Tracker @JvmOverloads constructor(
 	var packetsLost: Int? = null
 	var packetLoss: Float? = null
 	var customName: String? = null
+
+	/**
+	 * Please don't use this and instead set it via [Device.setMag]
+	 */
 	var magStatus: MagnetometerStatus = magStatus
-		private set
+		internal set
 
 	/**
 	 * Watch the rest calibration status
@@ -499,17 +503,6 @@ class Tracker @JvmOverloads constructor(
 	 */
 	fun isImu(): Boolean = imuType != null && trackerDataType == TrackerDataType.ROTATION
 
-	/**
-	 * Please don't use this and instead set it via [Device.setMag]
-	 */
-	internal fun setMagPrivate(mag: Boolean) {
-		magStatus = if (mag) {
-			MagnetometerStatus.ENABLED
-		} else {
-			MagnetometerStatus.DISABLED
-		}
-	}
-
 	/**
 	 * Gets the magnetic field vector, in mGauss.
 	 */

server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt

@@ -12,6 +12,7 @@ import io.eiren.util.logging.LogManager
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Quaternion.Companion.fromRotationVector
 import io.github.axisangles.ktmath.Vector3
+import kotlinx.coroutines.runBlocking
 import java.nio.ByteBuffer
 import java.util.function.Consumer
 import kotlin.collections.set
@@ -134,6 +135,10 @@ class HIDCommon {
 					.info(
 						"[TrackerServer] Added sensor $trackerId for ${device.name}, type $sensorType",
 					)
+			} else {
+				runBlocking {
+					device.setMag(magStatus, trackerId)
+				}
 			}
 		}
 

server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDDevice.kt

@@ -4,10 +4,16 @@ import dev.slimevr.tracking.trackers.Device
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.udp.BoardType
 import dev.slimevr.tracking.trackers.udp.MCUType
+import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
 
 class HIDDevice(val hidId: Int) : Device() {
 	override var hardwareIdentifier: String = "Unknown"
 	override var boardType: BoardType = BoardType.UNKNOWN
 	override var mcuType: MCUType = MCUType.UNKNOWN
 	fun getTracker(id: Int): Tracker? = trackers[id]
+
+	override suspend fun setMag(status: MagnetometerStatus, sensorId: Int) {
+		val tracker = trackers[sensorId] ?: error("Tracker $sensorId not found in device $this")
+		tracker.magStatus = status
+	}
 }

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt

@@ -249,7 +249,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 		) {
 			mainScope.launch {
 				withTimeoutOrNull(MAG_TIMEOUT) {
-					connection.setMag(false, trackerId)
+					connection.setMag(MagnetometerStatus.DISABLED, trackerId)
 				}
 			}
 		} else if (magStatus == MagnetometerStatus.DISABLED &&
@@ -258,7 +258,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 		) {
 			mainScope.launch {
 				withTimeoutOrNull(MAG_TIMEOUT) {
-					connection.setMag(true, trackerId)
+					connection.setMag(MagnetometerStatus.ENABLED, trackerId)
 				}
 			}
 		}

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/UDPDevice.kt

@@ -66,14 +66,15 @@ class UDPDevice(
 	var timedOut = false
 	override val trackers = ConcurrentHashMap<Int, Tracker>()
 
-	override suspend fun setMag(state: Boolean, sensorId: Int) {
+	override suspend fun setMag(magStatus: MagnetometerStatus, sensorId: Int) {
+		val state = magStatus == MagnetometerStatus.ENABLED
 		if (sensorId == 255) {
 			VRServer.instance.trackersServer.setConfigFlag(this, ConfigTypeId(1u), state)
-			trackers.forEach { (_, t) -> t.setMagPrivate(state) }
+			trackers.forEach { (_, t) -> t.magStatus = magStatus }
 		} else {
 			require(trackers[sensorId] != null) { "There is no tracker $sensorId in device ${toString()}" }
 			VRServer.instance.trackersServer.setConfigFlag(this, ConfigTypeId(1u), state, sensorId)
-			trackers[sensorId]!!.setMagPrivate(state)
+			trackers[sensorId]!!.magStatus = magStatus
 		}
 	}