impl: basic magnetometer toggling

lib/bno055_adafruit/Adafruit_BNO055.h

@@ -308,6 +308,8 @@ class Adafruit_BNO055 {
   void enterSuspendMode();
   void enterNormalMode();
 
+  adafruit_bno055_opmode_t getMode() { return _mode; }
+
 private:
   byte read8(adafruit_bno055_reg_t);
   bool readLen(adafruit_bno055_reg_t, byte *buffer, uint8_t len);

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

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");
 

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
 

src/sensors/bno055sensor.cpp

@@ -24,6 +24,27 @@
 #include "globals.h"
 #include "GlobalVars.h"
 
+/*
+	Info:
+
+	Check the datasheet for the BNO055 for more information on the different
+	operation modes.
+
+	OPERATION_MODE_IMUPLUS = OPR_MODE 0x08
+		In the IMU mode the relative orientation of the BNO055 in space is
+		calculated from the accelerometer and gyroscope data. The calculation
+		is fast.
+
+	OPERATION_MODE_NDOF = OPR_MODE 0x0C
+		This is a fusion mode with 9 degrees of freedom where the fused
+		absolute orientation data is calculated from accelerometer, gyroscope
+		and the magnetometer. The advantages of combining all three sensors are
+		a fast calculation, resulting in high output data rate, and high
+		robustness from magnetic field distortions. In this mode the
+		Fast Magnetometer calibration is turned ON and thereby resulting in
+		quick calibration of the magnetometer and higher output data accuracy.
+*/
+
 void BNO055Sensor::motionSetup() {
     imu = Adafruit_BNO055(sensorId, addr);
     delay(3000);
@@ -76,3 +97,13 @@ void BNO055Sensor::motionLoop() {
 void BNO055Sensor::startCalibration(int calibrationType) {
 
 }
+
+bool BNO055Sensor::hasMagnetometerEnabled() {
+    return imu.getMode() == Adafruit_BNO055::OPERATION_MODE_NDOF;
+}
+
+bool BNO055Sensor::toggleMagnetometer(bool enabled) {
+    imu.setMode(enabled ? Adafruit_BNO055::OPERATION_MODE_NDOF : Adafruit_BNO055::OPERATION_MODE_IMUPLUS);
+
+    return true;
+}

src/sensors/bno055sensor.h

@@ -38,6 +38,10 @@ class BNO055Sensor : public Sensor
     void motionLoop() override final;
     void startCalibration(int calibrationType) override final;
 
+    bool supportsTogglingMagnetometer() { return true; };
+    bool hasMagnetometerEnabled() override final;
+    bool toggleMagnetometer(bool enabled) override final;
+
 private:
     Adafruit_BNO055 imu;
 };

src/sensors/sensor.cpp

@@ -67,6 +67,12 @@ void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrati
 void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); };
 void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); };
 
+bool Sensor::toggleMagnetometer(bool enabled) {
+    m_Logger.error("Toggling the magnetometer is not supported or implemented for IMU %s", getIMUNameByType(sensorType));
+
+    return false;
+};
+
 const char * getIMUNameByType(int imuType) {
     switch(imuType) {
         case IMU_MPU9250:

src/sensors/sensor.h

@@ -66,6 +66,12 @@ class Sensor
     virtual void printDebugTemperatureCalibrationState();
     virtual void resetTemperatureCalibrationState();
     virtual void saveTemperatureCalibration();
+
+    // TODO: Replace with feature flags
+    virtual bool supportsTogglingMagnetometer() { return false; };
+    virtual bool hasMagnetometerEnabled() { return false; };
+    virtual bool toggleMagnetometer(bool enabled);
+
     bool isWorking() {
         return working;
     };
@@ -106,7 +112,7 @@ class Sensor
     Vector3 acceleration{};
 
     SlimeVR::Logging::Logger m_Logger;
-    
+
 public:
     uint8_t sclPin = 0;
     uint8_t sdaPin = 0;