impl(network): toggling mags

SlimeVR · Mar 8, 2024 · 2be2659 · 2be2659
1 parent a7e6e6a
commit 2be2659
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 0 deletions.
diff --git a/src/network/connection.cpp b/src/network/connection.cpp
@@ -299,6 +299,15 @@ void Connection::sendSensorInfo(Sensor* sensor) {
 	MUST(sendByte((uint8_t)sensor->getSensorState()));
 	MUST(sendByte(sensor->getSensorType()));
 
+	// 0b00000000
+	//         ^^
+	//         |`- supportsMagToggle
+	//         `-- hasMagEnabled
+	const bool supportsMagToggle = sensor->supportsTogglingMagnetometer();
+	const bool hasMagEnabled = sensor->hasMagnetometerEnabled();
+	const uint8_t magInfo = (supportsMagToggle << 1) | hasMagEnabled;
+	MUST(sendByte(magInfo));
+
 	MUST(endPacket());
 }
 
@@ -409,6 +418,19 @@ void Connection::sendTrackerDiscovery() {
 	MUST(endPacket());
 }
 
+void Connection::sendToggleMagnetometerResult(uint8_t sensorId, bool result) {
+	MUST(m_Connected);
+
+	MUST(beginPacket());
+
+	MUST(sendPacketType(PACKET_TOGGLE_MAGNETOMETER));
+	MUST(sendPacketNumber());
+	MUST(sendByte(sensorId));
+	MUST(sendByte(result));
+
+	MUST(endPacket());
+}
+
 #if ENABLE_INSPECTION
 void Connection::sendInspectionRawIMUData(
 	uint8_t sensorId,
@@ -709,6 +731,35 @@ void Connection::update() {
 				#endif
 			}
 
+			break;
+
+		case PACKET_TOGGLE_MAGNETOMETER:
+			constexpr size_t PACKET_TOGGLE_MAGNETOMETER_SIZE = 4 + 8 + 1 + 1;
+
+			// Packet type (4) + Packet number (8) + sensor ID (1) + enabled (1)
+			if (len < PACKET_TOGGLE_MAGNETOMETER_SIZE) {
+				m_Logger.warn("Invalid toggle magnetometer packet: too short");
+				break;
+			}
+
+			const auto sensorId = m_Packet[12];
+			const auto enabled = m_Packet[13];
+
+			const auto sensor = sensorManager.getSensors().at(sensorId);
+			if (sensor == nullptr) {
+				m_Logger.warn("Invalid sensor ID in toggle magnetometer packet");
+				break;
+			} 
+
+			bool result = false;
+			if (sensor->supportsTogglingMagnetometer()) {
+				result = sensor->toggleMagnetometer(enabled);
+			} else {
+				m_Logger.warn("Received toggle magnetometer packet for sensor that doesn't support it");
+			}
+
+			sendToggleMagnetometerResult(sensorId, result);
+
 			break;
 	}
 }

diff --git a/src/network/connection.h b/src/network/connection.h
@@ -158,6 +158,9 @@ class Connection {
 	// PACKET_SENSOR_INFO 15
 	void sendSensorInfo(Sensor* sensor);
 
+	// PACKET_TOGGLE_MAGNETOMETER 21
+	void sendToggleMagnetometerResult(uint8_t sensorId, bool result);
+
 	bool m_Connected = false;
 	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection");
 

diff --git a/src/network/packets.h b/src/network/packets.h
@@ -47,6 +47,7 @@
 #define PACKET_TEMPERATURE 20
 // #define PACKET_USER_ACTION 21 // Joycon buttons only currently
 #define PACKET_FEATURE_FLAGS 22
+#define PACKET_TOGGLE_MAGNETOMETER 23
 
 #define PACKET_BUNDLE 100