Softfusion cleanup (#424)

* Make SPI work

Move sensor building logic to a separate class and file

Don't use templates for RegisterInterface/I2CImpl/SPIImpl

This started getting ridiculous, now we can actually maintain it.

Make BNO085 work again

Add BNO to automatic detection, remove a bunch of others

Not all IMU types are enabled right now due to code size optimization, but it could be expanded in the future with optimization of Softfusion.

Pick IMU type automatically by asking it

* ESP32 spelling fix (#396)

Fix: ES32 -> ESP32

* (Probably) multiply acceleration by tracker rotation offset

* Remove stome stuff from softfusionsensor

* Missed stuff

* Undo defines changes

* Undo debug.h changes

* Uses32BitSensorData is unneccessary now

* Remove some unnecessary variables

* Cleanup some logging text

* Formatting

* Split SensorBuilder into a header-source file pair

* Fix copyright

* Fix some issues with glove after all the changes

Add definitions for SlimeVR v1.2
Fix typo and add comments to quaternion sandwich function

* Add NO_WIRE definition for SPI devices

* Fix formatting

* Minor fix

* If ICM-45686 not found, ask again nicely

* Fix MCP interface not building

* Remove uneccessary "default" ctor from SPIImpl

* Remove buildSensorReal

* Invert if statement in sensorbuilder+

* Fix formatting

* If ICM-45686 not found, ask again nicely

* Fix MCP interface not building

* Fix formatting

* Various cleanup

* Formattign

* Fix detected logic

* Remove unnecessary Self using

* For some reason this fixes memory corruption issues

* Formatting

* This actually works this time

* Small cleanup

* Remove some unused includes

* Formatting

* Mag support (Attempt 2) (#458)

* WhoAmI check working

* In theory this should be setting up the mag

* Not sure how that happened

* Add magnetometer status to GET TEST and GET INFO

* Formatting

* Formatting 2

---------

Co-authored-by: Eiren Rain <Eirenliel@users.noreply.github.com>
Co-authored-by: Butterscotch! <bscotchvanilla@gmail.com>

src/sensors/SensorBuilder.h

@@ -72,10 +72,10 @@
 
 #if USE_RUNTIME_CALIBRATION
 #include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
-#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator
+#define SFCALIBRATOR RuntimeCalibration::RuntimeCalibrator
 #else
 #include "sensors/softfusion/SoftfusionCalibration.h"
-#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator
+#define SFCALIBRATOR SoftfusionCalibrator
 #endif
 
 #ifdef ESP32

src/sensors/SensorToggles.cpp

@@ -25,3 +25,27 @@ bool SensorToggleState::getToggle(SensorToggles toggle) const {
 	}
 	return false;
 }
+
+void SensorToggleState::onToggleChange(
+	std::function<void(SensorToggles, bool)>&& callback
+) {
+	this->callback = callback;
+}
+
+void SensorToggleState::emitToggleChange(SensorToggles toggle, bool state) const {
+	if (callback) {
+		(*callback)(toggle, state);
+	}
+}
+
+const char* SensorToggleState::toggleToString(SensorToggles toggle) {
+	switch (toggle) {
+		case SensorToggles::MagEnabled:
+			return "MagEnabled";
+		case SensorToggles::CalibrationEnabled:
+			return "CalibrationEnabled";
+		case SensorToggles::TempGradientCalibrationEnabled:
+			return "TempGradientCalibrationEnabled";
+	}
+	return "Unknown";
+}

src/sensors/SensorToggles.h

@@ -24,6 +24,8 @@
 #pragma once
 
 #include <cstdint>
+#include <functional>
+#include <optional>
 
 #include "../debug.h"
 
@@ -38,7 +40,14 @@ public:
 	void setToggle(SensorToggles toggle, bool state);
 	[[nodiscard]] bool getToggle(SensorToggles toggle) const;
 
+	void onToggleChange(std::function<void(SensorToggles, bool)>&& callback);
+
+	static const char* toggleToString(SensorToggles toggle);
+
 private:
+	std::optional<std::function<void(SensorToggles, bool)>> callback;
+	void emitToggleChange(SensorToggles toggle, bool state) const;
+
 	bool magEnabled = !USE_6_AXIS;
 	bool calibrationEnabled = true;
 	bool tempGradientCalibrationEnabled = true;

src/sensors/bno080sensor.cpp

@@ -135,6 +135,13 @@ void BNO080Sensor::motionSetup() {
 	configured = true;
 	m_tpsCounter.reset();
 	m_dataCounter.reset();
+
+	toggles.onToggleChange([&](SensorToggles toggle, bool) {
+		if (toggle == SensorToggles::MagEnabled) {
+			// TODO: maybe handle this more gracefully, I'm sure it's possible
+			motionSetup();
+		}
+	});
 }
 
 void BNO080Sensor::motionLoop() {

src/sensors/sensor.cpp

@@ -91,6 +91,8 @@ void Sensor::resetTemperatureCalibrationState() {
 	printTemperatureCalibrationUnsupported();
 };
 
+const char* Sensor::getAttachedMagnetometer() const { return nullptr; }
+
 SlimeVR::Configuration::SensorConfigBits Sensor::getSensorConfigData() {
 	return SlimeVR::Configuration::SensorConfigBits{
 		.magEnabled = toggles.getToggle(SensorToggles::MagEnabled),
@@ -157,12 +159,16 @@ void Sensor::markRestCalibrationComplete(bool completed) {
 }
 
 void Sensor::setFlag(SensorToggles toggle, bool state) {
-	assert(isFlagSupported(toggle));
+	if (!isFlagSupported(toggle)) {
+		m_Logger.error(
+			"Toggle %s isn't supported by this sensor!",
+			SensorToggleState::toggleToString(toggle)
+		);
+		return;
+	}
 
 	toggles.setToggle(toggle, state);
 
 	configuration.setSensorToggles(sensorId, toggles);
 	configuration.save();
-
-	motionSetup();
 }

src/sensors/sensor.h

@@ -85,6 +85,11 @@ public:
 	virtual void printDebugTemperatureCalibrationState();
 	virtual void resetTemperatureCalibrationState();
 	virtual void saveTemperatureCalibration();
+	// TODO: currently only for softfusionsensor, bmi160 and others should get
+	// an overload too
+	virtual const char* getAttachedMagnetometer() const;
+	// TODO: realistically each sensor should print its own state instead of
+	// having 15 getters for things only the serial commands use
 	bool isWorking() { return working; };
 	bool getHadData() const { return hadData; };
 	bool isValid() { return m_hwInterface != nullptr; };

src/sensors/softfusion/CalibrationBase.h

@@ -28,12 +28,13 @@
 #include <functional>
 
 #include "configuration/SensorConfig.h"
+#include "imuconsts.h"
 #include "motionprocessing/types.h"
 #include "sensors/SensorFusion.h"
 
-namespace SlimeVR::Sensor {
+namespace SlimeVR::Sensors {
 
-template <typename IMU, typename RawSensorT, typename RawVectorT>
+template <typename IMU>
 class CalibrationBase {
 public:
 	CalibrationBase(
@@ -41,20 +42,17 @@ public:
 		IMU& sensor,
 		uint8_t sensorId,
 		SlimeVR::Logging::Logger& logger,
-		float TempTs,
-		float AScale,
-		float GScale,
 		SensorToggleState& toggles
 	)
 		: fusion{fusion}
 		, sensor{sensor}
 		, sensorId{sensorId}
 		, logger{logger}
-		, TempTs{TempTs}
-		, AScale{AScale}
-		, GScale{GScale}
 		, toggles{toggles} {}
 
+	using Consts = IMUConsts<IMU>;
+	using RawSensorT = typename Consts::RawSensorT;
+
 	static constexpr bool HasMotionlessCalib
 		= requires(IMU& i) { typename IMU::MotionlessCalibrationData; };
 	static constexpr size_t MotionlessCalibDataSize() {
@@ -65,15 +63,8 @@ public:
 		}
 	}
 
-	using EatSamplesFn = std::function<void(const uint32_t)>;
-	using ReturnLastFn
-		= std::function<std::tuple<RawVectorT, RawVectorT, int16_t>(const uint32_t)>;
-
-	virtual void startCalibration(
-		int calibrationType,
-		const EatSamplesFn& eatSamplesForSeconds,
-		const ReturnLastFn& eatSamplesReturnLast
-	){};
+	virtual void checkStartupCalibration() {}
+	virtual void startCalibration(int calibrationType){};
 
 	virtual bool calibrationMatches(
 		const SlimeVR::Configuration::SensorConfig& sensorCalibration
@@ -116,10 +107,7 @@ protected:
 	IMU& sensor;
 	uint8_t sensorId;
 	SlimeVR::Logging::Logger& logger;
-	float TempTs;
-	float AScale;
-	float GScale;
 	SensorToggleState& toggles;
 };
 
-}  // namespace SlimeVR::Sensor
+}  // namespace SlimeVR::Sensors

src/sensors/softfusion/SoftfusionCalibration.h

@@ -23,48 +23,120 @@
 
 #pragma once
 
+#include <cstdint>
 #include <cstring>
 #include <vector>
 
+#include "CalibrationBase.h"
 #include "GlobalVars.h"
 #include "configuration/SensorConfig.h"
 #include "logging/Logger.h"
 #include "motionprocessing/RestDetection.h"
 #include "motionprocessing/types.h"
 #include "sensors/SensorFusion.h"
-#include "sensors/softfusion/CalibrationBase.h"
 
-namespace SlimeVR::Sensor {
+namespace SlimeVR::Sensors {
 
-template <typename IMU, typename RawSensorT, typename RawVectorT>
-class SoftfusionCalibrator : public CalibrationBase<IMU, RawSensorT, RawVectorT> {
+template <typename IMU>
+class SoftfusionCalibrator : public CalibrationBase<IMU> {
 public:
 	static constexpr bool HasUpsideDownCalibration = true;
 
-	using Base = CalibrationBase<IMU, RawSensorT, RawVectorT>;
+	using Base = CalibrationBase<IMU>;
+	using Consts = typename Base::Consts;
+	using RawSensorT = typename Consts::RawSensorT;
+	using RawVectorT = typename Consts::RawVectorT;
 
 	SoftfusionCalibrator(
 		Sensors::SensorFusion& fusion,
 		IMU& sensor,
 		uint8_t sensorId,
 		SlimeVR::Logging::Logger& logger,
-		float TempTs,
-		float AScale,
-		float GScale,
 		SensorToggleState& toggles
 	)
-		: Base{fusion, sensor, sensorId, logger, TempTs, AScale, GScale, toggles} {
-		calibration.T_Ts = TempTs;
+		: Base{fusion, sensor, sensorId, logger, toggles} {
+		calibration.T_Ts = Consts::getDefaultTempTs();
 	}
 
-	void startCalibration(
-		int calibrationType,
-		const Base::EatSamplesFn& eatSamplesForSeconds,
-		const Base::ReturnLastFn& eatSamplesReturnLast
-	) final {
+	void eatSamplesForSeconds(const uint32_t seconds) {
+		const auto targetDelay = millis() + 1000 * seconds;
+		auto lastSecondsRemaining = seconds;
+		while (millis() < targetDelay) {
+#ifdef ESP8266
+			ESP.wdtFeed();
+#endif
+			auto currentSecondsRemaining = (targetDelay - millis()) / 1000;
+			if (currentSecondsRemaining != lastSecondsRemaining) {
+				logger.info("%d...", currentSecondsRemaining + 1);
+				lastSecondsRemaining = currentSecondsRemaining;
+			}
+
+			sensor.bulkRead({
+				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
+				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
+				[](const int16_t xyz, const sensor_real_t timeDelta) {},
+			});
+		}
+	}
+
+	std::tuple<RawVectorT, RawVectorT, int16_t> eatSamplesReturnLast(
+		const uint32_t milliseconds
+	) {
+		RawVectorT accel = {0};
+		RawVectorT gyro = {0};
+		int16_t temp = 0;
+		const auto targetDelay = millis() + milliseconds;
+		while (millis() < targetDelay) {
+			sensor.bulkRead({
+				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
+					accel[0] = xyz[0];
+					accel[1] = xyz[1];
+					accel[2] = xyz[2];
+				},
+				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
+					gyro[0] = xyz[0];
+					gyro[1] = xyz[1];
+					gyro[2] = xyz[2];
+				},
+				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
+					temp = rawTemp;
+				},
+			});
+			yield();
+		}
+		return std::make_tuple(accel, gyro, temp);
+	}
+
+	void checkStartupCalibration() final {
+		auto lastRawSample = eatSamplesReturnLast(1000);
+		auto gravity = static_cast<sensor_real_t>(
+			Consts::AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
+		);
+		logger.info("Gravity read: %.1f (need < -7.5 to start calibration)", gravity);
+		if (gravity > -7.5f) {
+			return;
+		}
+
+		ledManager.on();
+		logger.info("Flip front in 5 seconds to start calibration");
+		lastRawSample = eatSamplesReturnLast(5000);
+		gravity = static_cast<sensor_real_t>(
+			Consts::AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
+		);
+		if (gravity > 7.5f) {
+			logger.debug("Starting calibration...");
+			startCalibration(0);
+		} else {
+			logger.info("Flip not detected. Skipping calibration.");
+		}
+
+		ledManager.off();
+	}
+
+	void startCalibration(int calibrationType) final {
 		if (calibrationType == 0) {
 			// ALL
-			calibrateSampleRate(eatSamplesForSeconds);
+			calibrateSampleRate();
 			if constexpr (Base::HasMotionlessCalib) {
 				typename IMU::MotionlessCalibrationData calibData;
 				sensor.motionlessCalibration(calibData);
@@ -73,14 +145,14 @@ public:
 			// Gryoscope offset calibration can only happen after any motionless
 			// gyroscope calibration, otherwise we are calculating the offset based
 			// on an incorrect starting point
-			calibrateGyroOffset(eatSamplesReturnLast);
-			calibrateAccel(eatSamplesForSeconds);
+			calibrateGyroOffset();
+			calibrateAccel();
 		} else if (calibrationType == 1) {
-			calibrateSampleRate(eatSamplesForSeconds);
+			calibrateSampleRate();
 		} else if (calibrationType == 2) {
-			calibrateGyroOffset(eatSamplesReturnLast);
+			calibrateGyroOffset();
 		} else if (calibrationType == 3) {
-			calibrateAccel(eatSamplesForSeconds);
+			calibrateAccel();
 		} else if (calibrationType == 4) {
 			if constexpr (Base::HasMotionlessCalib) {
 				typename IMU::MotionlessCalibrationData calibData;
@@ -133,28 +205,28 @@ public:
 		accelSample[0]
 			= (calibration.A_Ainv[0][0] * tmp[0] + calibration.A_Ainv[0][1] * tmp[1]
 			   + calibration.A_Ainv[0][2] * tmp[2])
-			* AScale;
+			* Consts::AScale;
 		accelSample[1]
 			= (calibration.A_Ainv[1][0] * tmp[0] + calibration.A_Ainv[1][1] * tmp[1]
 			   + calibration.A_Ainv[1][2] * tmp[2])
-			* AScale;
+			* Consts::AScale;
 		accelSample[2]
 			= (calibration.A_Ainv[2][0] * tmp[0] + calibration.A_Ainv[2][1] * tmp[1]
 			   + calibration.A_Ainv[2][2] * tmp[2])
-			* AScale;
+			* Consts::AScale;
 	}
 
 	float getAccelTimestep() final { return calibration.A_Ts; }
 
 	void scaleGyroSample(sensor_real_t gyroSample[3]) final {
 		gyroSample[0] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[0] - calibration.G_off[0])
+			Consts::GScale * (gyroSample[0] - calibration.G_off[0])
 		);
 		gyroSample[1] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[1] - calibration.G_off[1])
+			Consts::GScale * (gyroSample[1] - calibration.G_off[1])
 		);
 		gyroSample[2] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[2] - calibration.G_off[2])
+			Consts::GScale * (gyroSample[2] - calibration.G_off[2])
 		);
 	}
 
@@ -185,15 +257,15 @@ private:
 		configuration.save();
 	}
 
-	void calibrateGyroOffset(const Base::ReturnLastFn& eatSamplesReturnLast) {
+	void calibrateGyroOffset() {
 		if (!toggles.getToggle(SensorToggles::CalibrationEnabled)) {
 			return;
 		}
 
 		// Wait for sensor to calm down before calibration
 		logger.info(
-			"Put down the device and wait for baseline gyro reading calibration (%d "
-			"seconds)",
+			"Put down the device and wait for baseline gyro reading calibration "
+			"(%d seconds)",
 			GyroCalibDelaySeconds
 		);
 		ledManager.on();
@@ -216,7 +288,7 @@ private:
 #ifdef ESP8266
 			ESP.wdtFeed();
 #endif
-			sensor.bulkRead(
+			sensor.bulkRead({
 				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
 				[&sumXYZ,
 				 &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
@@ -225,8 +297,8 @@ private:
 					sumXYZ[2] += xyz[2];
 					++sampleCount;
 				},
-				[](const int16_t rawTemp, const sensor_real_t timeDelta) {}
-			);
+				[](const int16_t rawTemp, const sensor_real_t timeDelta) {},
+			});
 		}
 
 		ledManager.off();
@@ -244,15 +316,15 @@ private:
 		);
 	}
 
-	void calibrateAccel(const Base::EatSamplesFn& eatSamplesForSeconds) {
+	void calibrateAccel() {
 		if (!toggles.getToggle(SensorToggles::CalibrationEnabled)) {
 			return;
 		}
 
 		auto magneto = std::make_unique<MagnetoCalibration>();
 		logger.info(
-			"Put the device into 6 unique orientations (all sides), leave it still and "
-			"do not hold/touch for %d seconds each",
+			"Put the device into 6 unique orientations (all sides), leave it still "
+			"and do not hold/touch for %d seconds each",
 			AccelCalibRestSeconds
 		);
 		ledManager.on();
@@ -290,17 +362,17 @@ private:
 #ifdef ESP8266
 			ESP.wdtFeed();
 #endif
-			sensor.bulkRead(
+			sensor.bulkRead({
 				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
 					const sensor_real_t scaledData[]
 						= {static_cast<sensor_real_t>(
-							   AScale * static_cast<sensor_real_t>(xyz[0])
+							   Consts::AScale * static_cast<sensor_real_t>(xyz[0])
 						   ),
 						   static_cast<sensor_real_t>(
-							   AScale * static_cast<sensor_real_t>(xyz[1])
+							   Consts::AScale * static_cast<sensor_real_t>(xyz[1])
 						   ),
 						   static_cast<sensor_real_t>(
-							   AScale * static_cast<sensor_real_t>(xyz[2])
+							   Consts::AScale * static_cast<sensor_real_t>(xyz[2])
 						   )};
 
 					calibrationRestDetection.updateAcc(IMU::AccTs, scaledData);
@@ -347,8 +419,8 @@ private:
 					}
 				},
 				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
-				[](const int16_t rawTemp, const sensor_real_t timeDelta) {}
-			);
+				[](const int16_t rawTemp, const sensor_real_t timeDelta) {},
+			});
 		}
 		ledManager.off();
 		logger.debug("Calculating accelerometer calibration data...");
@@ -376,7 +448,7 @@ private:
 		logger.debug("}");
 	}
 
-	void calibrateSampleRate(const Base::EatSamplesFn& eatSamplesForSeconds) {
+	void calibrateSampleRate() {
 		logger.debug(
 			"Calibrating IMU sample rate in %d second(s)...",
 			SampleRateCalibDelaySeconds
@@ -392,7 +464,7 @@ private:
 		logger.debug("Counting samples now...");
 		uint32_t currentTime;
 		while ((currentTime = millis()) < calibTarget) {
-			sensor.bulkRead(
+			sensor.bulkRead({
 				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
 					accelSamples++;
 				},
@@ -401,8 +473,8 @@ private:
 				},
 				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
 					tempSamples++;
-				}
-			);
+				},
+			});
 			yield();
 		}
 
@@ -423,7 +495,8 @@ private:
 			= millisFromStart / (static_cast<float>(tempSamples) * 1000.0f);
 
 		logger.debug(
-			"Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: %fHz",
+			"Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: "
+			"%fHz",
 			1.0 / calibration.G_Ts,
 			1.0 / calibration.A_Ts,
 			1.0 / calibration.T_Ts
@@ -453,12 +526,10 @@ private:
 	};
 
 private:
-	using Base::AScale;
-	using Base::GScale;
 	using Base::logger;
 	using Base::sensor;
 	using Base::sensorId;
 	using Base::toggles;
 };
 
-}  // namespace SlimeVR::Sensor
+}  // namespace SlimeVR::Sensors

src/sensors/softfusion/drivers/bmi270.h

@@ -31,6 +31,7 @@
 
 #include "../../../sensorinterface/RegisterInterface.h"
 #include "bmi270fw.h"
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -433,12 +434,7 @@ struct BMI270 {
 		return to_ret;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
+	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
 		const auto bytes_to_read = std::min(
@@ -480,7 +476,7 @@ struct BMI270 {
 						static_cast<int32_t>(ShortLimit::min()),
 						static_cast<int32_t>(ShortLimit::max())
 					);
-					processGyroSample(gyro, GyrTs);
+					callbacks.processGyroSample(gyro, GyrTs);
 				}
 
 				if (header & Fifo::AccelDataBit) {
@@ -488,7 +484,7 @@ struct BMI270 {
 					accel[0] = getFromFifo<uint16_t>(i, read_buffer);
 					accel[1] = getFromFifo<uint16_t>(i, read_buffer);
 					accel[2] = getFromFifo<uint16_t>(i, read_buffer);
-					processAccelSample(accel, AccTs);
+					callbacks.processAccelSample(accel, AccTs);
 				}
 			}
 		}

src/sensors/softfusion/drivers/callbacks.h

@@ -0,0 +1,34 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+#include <functional>
+
+template <typename SampleType>
+struct DriverCallbacks {
+	std::function<void(const SampleType sample[3], float AccTs)> processAccelSample;
+	std::function<void(const SampleType sample[3], float GyrTs)> processGyroSample;
+	std::function<void(int16_t sample, float TempTs)> processTempSample;
+};

src/sensors/softfusion/drivers/icm42688.h

@@ -27,6 +27,7 @@
 #include <array>
 #include <cstdint>
 
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -50,8 +51,6 @@ struct ICM42688 {
 	static constexpr float GyroSensitivity = 32.8f;
 	static constexpr float AccelSensitivity = 4096.0f;
 
-	static constexpr bool Uses32BitSensorData = true;
-
 	static constexpr float TemperatureBias = 25.0f;
 	static constexpr float TemperatureSensitivity = 2.07f;
 
@@ -159,12 +158,7 @@ struct ICM42688 {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTemperatureSample
-	) {
+	void bulkRead(DriverCallbacks<int32_t>&& callbacks) {
 		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
 		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
@@ -188,7 +182,7 @@ struct ICM42688 {
 				static_cast<int32_t>(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf),
 				static_cast<int32_t>(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf),
 			};
-			processGyroSample(gyroData, GyrTs);
+			callbacks.processGyroSample(gyroData, GyrTs);
 
 			if (entry.part.accel[0] != -32768) {
 				const int32_t accelData[3]{
@@ -199,11 +193,14 @@ struct ICM42688 {
 					static_cast<int32_t>(entry.part.accel[2]) << 4
 						| (static_cast<int32_t>(entry.part.zlsb) & 0xf0 >> 4),
 				};
-				processAccelSample(accelData, AccTs);
+				callbacks.processAccelSample(accelData, AccTs);
 			}
 
 			if (entry.part.temp != 0x8000) {
-				processTemperatureSample(static_cast<int16_t>(entry.part.temp), TempTs);
+				callbacks.processTempSample(
+					static_cast<int16_t>(entry.part.temp),
+					TempTs
+				);
 			}
 		}
 	}

src/sensors/softfusion/drivers/icm45base.h

@@ -25,6 +25,8 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
+#include "sensors/softfusion/magdriver.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
@@ -52,8 +54,6 @@ struct ICM45Base {
 
 	static constexpr float TemperatureZROChange = 20.0f;
 
-	static constexpr bool Uses32BitSensorData = true;
-
 	RegisterInterface& m_RegisterInterface;
 	SlimeVR::Logging::Logger& m_Logger;
 	ICM45Base(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
@@ -105,6 +105,77 @@ struct ICM45Base {
 
 		static constexpr uint8_t FifoCount = 0x12;
 		static constexpr uint8_t FifoData = 0x14;
+
+		// Indirect Register Access
+
+		static constexpr uint32_t IRegWaitTimeMicros = 4;
+
+		enum class Bank : uint8_t {
+			IMemSram = 0x00,
+			IPregBar = 0xa0,
+			IPregSys1 = 0xa4,
+			IPregSys2 = 0xa5,
+			IPregTop1 = 0xa2,
+		};
+
+		static constexpr uint8_t IRegAddr = 0x7c;
+		static constexpr uint8_t IRegData = 0x7e;
+
+		// Mag Support
+
+		struct IOCPadScenarioAuxOvrd {
+			static constexpr uint8_t reg = 0x30;
+			static constexpr uint8_t value = (0b1 << 4)  // Enable AUX1 override
+										   | (0b01 << 2)  // Enable I2CM master
+										   | (0b1 << 1)  // Enable AUX1 enable override
+										   | (0b1 << 0);  // Enable AUX1
+		};
+
+		struct I2CMCommand0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x06;
+		};
+
+		struct I2CMDevProfile0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x0e;
+		};
+
+		struct I2CMDevProfile1 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x0f;
+		};
+
+		struct I2CMWrData0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x33;
+		};
+
+		struct I2CMRdData0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x1b;
+		};
+
+		struct DmpExtSenOdrCfg {
+			// TODO: todo
+		};
+
+		struct I2CMControl {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x16;
+		};
+
+		struct I2CMStatus {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x18;
+
+			static constexpr uint8_t SDAErr = 0b1 << 5;
+			static constexpr uint8_t SCLErr = 0b1 << 4;
+			static constexpr uint8_t SRSTErr = 0b1 << 3;
+			static constexpr uint8_t TimeoutErr = 0b1 << 2;
+			static constexpr uint8_t Done = 0b1 << 1;
+			static constexpr uint8_t Busy = 0b1 << 0;
+		};
 	};
 
 #pragma pack(push, 1)
@@ -149,51 +220,50 @@ struct ICM45Base {
 			BaseRegs::PwrMgmt0::reg,
 			BaseRegs::PwrMgmt0::value
 		);
+
+		m_RegisterInterface.writeReg(
+			BaseRegs::IOCPadScenarioAuxOvrd::reg,
+			BaseRegs::IOCPadScenarioAuxOvrd::value
+		);
+
+		read_buffer.resize(FullFifoEntrySize * MaxReadings);
+
 		delay(1);
 
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTemperatureSample
-	) {
-		// Allocate statically so that it does not take up stack space, which
-		// can result in stack overflow and panic
-		constexpr size_t MaxReadings = 8;
-		static std::array<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
+	static constexpr size_t MaxReadings = 8;
+	// Allocate on heap so that it does not take up stack space, which can result in
+	// stack overflow and panic
+	std::vector<uint8_t> read_buffer;
 
+	void bulkRead(DriverCallbacks<int32_t>&& callbacks) {
 		constexpr int16_t InvalidReading = -32768;
 
 		size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount);
 
-		if (fifo_packets >= 1) {
-			//
-			// AN-000364
-			// 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA
-			//
-			// Description: When in FIFO streaming mode, a FIFO empty event
-			// (caused by host reading the last byte of the last FIFO frame) can
-			// cause FIFO data corruption in the first FIFO frame that arrives
-			// after the FIFO empty condition. Once the issue is triggered, the
-			// FIFO state is compromised and cannot recover. FIFO must be set in
-			// bypass mode to flush out the wrong state
-			//
-			// When operating in FIFO streaming mode, if FIFO threshold
-			// interrupt is triggered with M number of FIFO frames accumulated
-			// in the FIFO buffer, the host should only read the first M-1
-			// number of FIFO frames. This prevents the FIFO empty event, that
-			// can cause FIFO data corruption, from happening.
-			//
-			--fifo_packets;
-		}
-
-		if (fifo_packets == 0) {
+		if (fifo_packets <= 1) {
 			return;
 		}
 
+		// AN-000364
+		// 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA
+		//
+		// Description: When in FIFO streaming mode, a FIFO empty event
+		// (caused by host reading the last byte of the last FIFO frame) can
+		// cause FIFO data corruption in the first FIFO frame that arrives
+		// after the FIFO empty condition. Once the issue is triggered, the
+		// FIFO state is compromised and cannot recover. FIFO must be set in
+		// bypass mode to flush out the wrong state
+		//
+		// When operating in FIFO streaming mode, if FIFO threshold
+		// interrupt is triggered with M number of FIFO frames accumulated
+		// in the FIFO buffer, the host should only read the first M-1
+		// number of FIFO frames. This prevents the FIFO empty event, that
+		// can cause FIFO data corruption, from happening.
+		--fifo_packets;
+
 		fifo_packets = std::min(fifo_packets, MaxReadings);
 
 		size_t bytes_to_read = fifo_packets * FullFifoEntrySize;
@@ -218,7 +288,7 @@ struct ICM45Base {
 					static_cast<int32_t>(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf),
 					static_cast<int32_t>(entry.gyro[2]) << 4 | (entry.lsb[2] & 0xf),
 				};
-				processGyroSample(gyroData, GyrTs);
+				callbacks.processGyroSample(gyroData, GyrTs);
 			}
 
 			if (has_accel && entry.accel[0] != InvalidReading) {
@@ -230,14 +300,138 @@ struct ICM45Base {
 					static_cast<int32_t>(entry.accel[2]) << 4
 						| (static_cast<int32_t>((entry.lsb[2]) & 0xf0) >> 4),
 				};
-				processAccelSample(accelData, AccTs);
+				callbacks.processAccelSample(accelData, AccTs);
 			}
 
 			if (entry.temp != 0x8000) {
-				processTemperatureSample(static_cast<int16_t>(entry.temp), TempTs);
+				callbacks.processTempSample(static_cast<int16_t>(entry.temp), TempTs);
 			}
 		}
 	}
+
+	template <typename Reg>
+	uint8_t readBankRegister() {
+		uint8_t buffer;
+		readBankRegister<Reg>(&buffer, sizeof(buffer));
+		return buffer;
+	}
+
+	template <typename Reg, typename T>
+	void readBankRegister(T* buffer, size_t length) {
+		uint8_t data[] = {
+			static_cast<uint8_t>(Reg::bank),
+			Reg::reg,
+		};
+
+		auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
+		m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
+		delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		for (size_t i = 0; i < length * sizeof(T); i++) {
+			bufferBytes[i] = m_RegisterInterface.readReg(BaseRegs::IRegData);
+			delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		}
+	}
+
+	template <typename Reg>
+	void writeBankRegister() {
+		writeBankRegister<Reg>(&Reg::value, sizeof(Reg::value));
+	}
+
+	template <typename Reg, typename T>
+	void writeBankRegister(T* buffer, size_t length) {
+		auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
+
+		uint8_t data[] = {
+			static_cast<uint8_t>(Reg::bank),
+			Reg::reg,
+			bufferBytes[0],
+		};
+
+		m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
+		delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		for (size_t i = 1; i < length * sizeof(T); i++) {
+			m_RegisterInterface.writeReg(BaseRegs::IRegData, bufferBytes[i]);
+			delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		}
+	}
+
+	template <typename Reg>
+	void writeBankRegister(uint8_t value) {
+		writeBankRegister<Reg>(&value, sizeof(value));
+	}
+
+	void setAuxId(uint8_t deviceId) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile1>(deviceId);
+	}
+
+	uint8_t readAux(uint8_t address) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
+
+		writeBankRegister<typename BaseRegs::I2CMCommand0>(
+			(0b1 << 7)  // Last transaction
+			| (0b0 << 6)  // Channel 0
+			| (0b01 << 4)  // Read with register
+			| (0b0001 << 0)  // Read 1 byte
+		);
+		writeBankRegister<typename BaseRegs::I2CMControl>(
+			(0b0 << 6)  // No restarts
+			| (0b0 << 3)  // Fast mode
+			| (0b1 << 0)  // Start transaction
+		);
+
+		uint8_t lastStatus;
+		while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
+			   & BaseRegs::I2CMStatus::Busy)
+			;
+
+		if (lastStatus != BaseRegs::I2CMStatus::Done) {
+			m_Logger.error(
+				"Aux read from address 0x%02x returned status 0x%02x",
+				address,
+				lastStatus
+			);
+		}
+
+		return readBankRegister<typename BaseRegs::I2CMRdData0>();
+	}
+
+	void writeAux(uint8_t address, uint8_t value) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
+		writeBankRegister<typename BaseRegs::I2CMWrData0>(value);
+		writeBankRegister<typename BaseRegs::I2CMCommand0>(
+			(0b1 << 7)  // Last transaction
+			| (0b0 << 6)  // Channel 0
+			| (0b01 << 4)  // Read with register
+			| (0b0001 << 0)  // Read 1 byte
+		);
+		writeBankRegister<typename BaseRegs::I2CMControl>(
+			(0b0 << 6)  // No restarts
+			| (0b0 << 3)  // Fast mode
+			| (0b1 << 0)  // Start transaction
+		);
+
+		uint8_t lastStatus;
+		while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
+			   & BaseRegs::I2CMStatus::Busy)
+			;
+
+		if (lastStatus != BaseRegs::I2CMStatus::Done) {
+			m_Logger.error(
+				"Aux write to address 0x%02x with value 0x%02x returned status 0x%02x",
+				address,
+				value,
+				lastStatus
+			);
+		}
+	}
+
+	void startAuxPolling(uint8_t dataReg, MagDataWidth dataWidth) {
+		// TODO:
+	}
+
+	void stopAuxPolling() {
+		// TODO:
+	}
 };
 
 };  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/lsm6ds-common.h

@@ -28,6 +28,7 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
@@ -53,11 +54,9 @@ struct LSM6DSOutputHandler {
 
 	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
 
-	template <typename AccelCall, typename GyroCall, typename TempCall, typename Regs>
+	template <typename Regs>
 	void bulkRead(
-		AccelCall& processAccelSample,
-		GyroCall& processGyroSample,
-		TempCall& processTempSample,
+		DriverCallbacks<int16_t>&& callbacks,
 		float GyrTs,
 		float AccTs,
 		float TempTs
@@ -94,13 +93,13 @@ struct LSM6DSOutputHandler {
 
 			switch (tag) {
 				case 0x01:  // Gyro NC
-					processGyroSample(entry.xyz, GyrTs);
+					callbacks.processGyroSample(entry.xyz, GyrTs);
 					break;
 				case 0x02:  // Accel NC
-					processAccelSample(entry.xyz, AccTs);
+					callbacks.processAccelSample(entry.xyz, AccTs);
 					break;
 				case 0x03:  // Temperature
-					processTempSample(entry.xyz[0], TempTs);
+					callbacks.processTempSample(entry.xyz[0], TempTs);
 					break;
 			}
 		}

src/sensors/softfusion/drivers/lsm6ds3trc.h

@@ -28,6 +28,7 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -123,12 +124,7 @@ struct LSM6DS3TRC {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTemperatureSample
-	) {
+	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto read_result = m_RegisterInterface.readReg16(Regs::FifoStatus);
 		if (read_result & 0x4000) {  // overrun!
 			// disable and re-enable fifo to clear it
@@ -158,14 +154,11 @@ struct LSM6DS3TRC {
 		);
 		for (uint16_t i = 0; i < bytes_to_read / sizeof(uint16_t);
 			 i += single_measurement_words) {
-			processGyroSample(reinterpret_cast<const int16_t*>(&read_buffer[i]), GyrTs);
-			processAccelSample(
-				reinterpret_cast<const int16_t*>(&read_buffer[i + 3]),
-				AccTs
-			);
-			processTemperatureSample(read_buffer[i + 9], TempTs);
+			callbacks.processGyroSample(&read_buffer[i], GyrTs);
+			callbacks.processAccelSample(&read_buffer[i + 3], AccTs);
+			callbacks.processTempSample(read_buffer[i + 9], TempTs);
 		}
 	}
-};
+};  // namespace SlimeVR::Sensors::SoftFusion::Drivers
 
 }  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/drivers/lsm6dso.h

@@ -27,6 +27,7 @@
 #include <array>
 #include <cstdint>
 
+#include "callbacks.h"
 #include "lsm6ds-common.h"
 #include "vqf.h"
 
@@ -122,16 +123,9 @@ struct LSM6DSO : LSM6DSOutputHandler {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
-		LSM6DSOutputHandler::template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-			processAccelSample,
-			processGyroSample,
-			processTempSample,
+	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		LSM6DSOutputHandler::template bulkRead<Regs>(
+			std::move(callbacks),
 			GyrTs,
 			AccTs,
 			TempTs

src/sensors/softfusion/drivers/lsm6dsr.h

@@ -121,16 +121,9 @@ struct LSM6DSR : LSM6DSOutputHandler {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
-		LSM6DSOutputHandler::template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-			processAccelSample,
-			processGyroSample,
-			processTempSample,
+	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		LSM6DSOutputHandler::template bulkRead<Regs>(
+			std::move(callbacks),
 			GyrTs,
 			AccTs,
 			TempTs

src/sensors/softfusion/drivers/lsm6dsv.h

@@ -137,16 +137,9 @@ struct LSM6DSV : LSM6DSOutputHandler {
 		return true;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
-		LSM6DSOutputHandler::bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-			processAccelSample,
-			processGyroSample,
-			processTempSample,
+	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
+		LSM6DSOutputHandler::template bulkRead<Regs>(
+			std::move(callbacks),
 			GyrTs,
 			AccTs,
 			TempTs

src/sensors/softfusion/drivers/mpu6050.h

@@ -30,6 +30,7 @@
 #include <cstdint>
 
 #include "../../../sensorinterface/RegisterInterface.h"
+#include "callbacks.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -181,12 +182,7 @@ struct MPU6050 {
 		return result;
 	}
 
-	template <typename AccelCall, typename GyroCall, typename TempCall>
-	void bulkRead(
-		AccelCall&& processAccelSample,
-		GyroCall&& processGyroSample,
-		TempCall&& processTempSample
-	) {
+	void bulkRead(DriverCallbacks<int16_t>&& callbacks) {
 		const auto status = m_RegisterInterface.readReg(Regs::IntStatus);
 
 		if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) {
@@ -216,14 +212,14 @@ struct MPU6050 {
 			xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x);
 			xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y);
 			xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z);
-			processAccelSample(xyz, AccTs);
+			callbacks.processAccelSample(xyz, AccTs);
 
 			xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x);
 			xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y);
 			xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z);
-			processGyroSample(xyz, GyrTs);
+			callbacks.processGyroSample(xyz, GyrTs);
 		}
 	}
-};
+};  // namespace SlimeVR::Sensors::SoftFusion::Drivers
 
 }  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/imuconsts.h

@@ -0,0 +1,68 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <array>
+#include <cstdint>
+#include <type_traits>
+
+#include "../../motionprocessing/types.h"
+#include "drivers/callbacks.h"
+
+template <typename IMU>
+struct IMUConsts {
+	static constexpr bool Uses32BitSensorData
+		= requires(IMU& i, DriverCallbacks<int32_t> callbacks) {
+			  i.bulkRead(std::move(callbacks));
+		  };
+
+	static constexpr bool DirectTempReadOnly = requires(IMU& i) { i.getDirectTemp(); };
+
+	using RawSensorT =
+		typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
+	using RawVectorT = std::array<RawSensorT, 3>;
+
+	static constexpr float GScale
+		= ((32768. / IMU::GyroSensitivity) / 32768.) * (PI / 180.0);
+	static constexpr float AScale = CONST_EARTH_GRAVITY / IMU::AccelSensitivity;
+
+	static constexpr float DirectTempReadFreq = 15;
+	static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq;
+	static constexpr sensor_real_t getDefaultTempTs() {
+		if constexpr (DirectTempReadOnly) {
+			return DirectTempReadTs;
+		} else {
+			return IMU::TempTs;
+		}
+	}
+
+	static constexpr bool SupportsMags = requires(IMU& i) { i.readAux(0x00); };
+	static constexpr bool Supports9ByteMag = []() constexpr {
+		if constexpr (requires { IMU::Supports9ByteMag; }) {
+			return IMU::Supports9ByteMag;
+		} else {
+			return true;
+		}
+	}();
+};

src/sensors/softfusion/magdriver.cpp

@@ -0,0 +1,132 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "magdriver.h"
+
+namespace SlimeVR::Sensors::SoftFusion {
+
+std::vector<MagDefinition> MagDriver::supportedMags{
+	MagDefinition{
+		.name = "QMC6309",
+
+		.deviceId = 0x7c,
+
+		.whoAmIReg = 0x00,
+		.expectedWhoAmI = 0x90,
+
+		.dataWidth = MagDataWidth::SixByte,
+		.dataReg = 0x01,
+
+		.setup =
+			[](MagInterface& interface) {
+				interface.writeByte(0x0b, 0x80);
+				interface.writeByte(0x0b, 0x00);  // Soft reset
+				delay(10);
+				interface.writeByte(0x0b, 0x48);  // Set/reset on, 8g full range, 200Hz
+				interface.writeByte(
+					0x0a,
+					0x21
+				);  // LP filter 2, 8x Oversampling, normal mode
+				return true;
+			},
+	},
+	MagDefinition{
+		.name = "IST8306",
+
+		.deviceId = 0x19,
+
+		.whoAmIReg = 0x00,
+		.expectedWhoAmI = 0x06,
+
+		.dataWidth = MagDataWidth::SixByte,
+		.dataReg = 0x11,
+
+		.setup =
+			[](MagInterface& interface) {
+				interface.writeByte(0x32, 0x01);  // Soft reset
+				delay(50);
+				interface.writeByte(0x30, 0x20);  // Noise suppression: low
+				interface.writeByte(0x41, 0x2d);  // Oversampling: 32X
+				interface.writeByte(0x31, 0x02);  // Continuous measurement @ 10Hz
+				return true;
+			},
+	},
+};
+
+bool MagDriver::init(MagInterface&& interface, bool supports9ByteMags) {
+	for (auto& mag : supportedMags) {
+		interface.setDeviceId(mag.deviceId);
+
+		logger.info("Trying mag %s!", mag.name);
+
+		uint8_t whoAmI = interface.readByte(mag.whoAmIReg);
+		if (whoAmI != mag.expectedWhoAmI) {
+			continue;
+		}
+
+		if (!supports9ByteMags && mag.dataWidth == MagDataWidth::NineByte) {
+			logger.error("The sensor doesn't support this mag!");
+			return false;
+		}
+
+		logger.info("Found mag %s! Initializing", mag.name);
+
+		if (!mag.setup(interface)) {
+			logger.error("Mag %s failed to initialize!", mag.name);
+			return false;
+		}
+
+		detectedMag = mag;
+
+		break;
+	}
+
+	this->interface = interface;
+	return detectedMag.has_value();
+}
+
+void MagDriver::startPolling() const {
+	if (!detectedMag) {
+		return;
+	}
+
+	interface.startPolling(detectedMag->dataReg, detectedMag->dataWidth);
+}
+
+void MagDriver::stopPolling() const {
+	if (!detectedMag) {
+		return;
+	}
+
+	interface.stopPolling();
+}
+
+const char* MagDriver::getAttachedMagName() const {
+	if (!detectedMag) {
+		return nullptr;
+	}
+
+	return detectedMag->name;
+}
+
+}  // namespace SlimeVR::Sensors::SoftFusion

src/sensors/softfusion/magdriver.h

@@ -0,0 +1,77 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <functional>
+#include <optional>
+
+#include "logging/Logger.h"
+#include "sensorinterface/RegisterInterface.h"
+
+namespace SlimeVR::Sensors::SoftFusion {
+
+enum class MagDataWidth {
+	SixByte,
+	NineByte,
+};
+
+struct MagInterface {
+	std::function<uint8_t(uint8_t)> readByte;
+	std::function<void(uint8_t, uint8_t)> writeByte;
+	std::function<void(uint8_t)> setDeviceId;
+	std::function<void(uint8_t, MagDataWidth)> startPolling;
+	std::function<void()> stopPolling;
+};
+
+struct MagDefinition {
+	const char* name;
+
+	uint8_t deviceId;
+
+	uint8_t whoAmIReg;
+	uint8_t expectedWhoAmI;
+
+	MagDataWidth dataWidth;
+	uint8_t dataReg;
+
+	std::function<bool(MagInterface& interface)> setup;
+};
+
+class MagDriver {
+public:
+	bool init(MagInterface&& interface, bool supports9ByteMags);
+	void startPolling() const;
+	void stopPolling() const;
+	[[nodiscard]] const char* getAttachedMagName() const;
+
+private:
+	std::optional<MagDefinition> detectedMag;
+	MagInterface interface;
+
+	static std::vector<MagDefinition> supportedMags;
+
+	Logging::Logger logger{"MagDriver"};
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion

src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h

@@ -41,27 +41,27 @@
 
 namespace SlimeVR::Sensors::RuntimeCalibration {
 
-template <typename IMU, typename RawSensorT, typename RawVectorT>
-class RuntimeCalibrator : public Sensor::CalibrationBase<IMU, RawSensorT, RawVectorT> {
+template <typename IMU>
+class RuntimeCalibrator : public Sensors::CalibrationBase<IMU> {
 public:
 	static constexpr bool HasUpsideDownCalibration = false;
 
-	using Base = Sensor::CalibrationBase<IMU, RawSensorT, RawVectorT>;
-	using Self = RuntimeCalibrator<IMU, RawSensorT, RawVectorT>;
+	using Base = Sensors::CalibrationBase<IMU>;
+	using Self = RuntimeCalibrator<IMU>;
+	using Consts = typename Base::Consts;
+	using RawSensorT = typename Consts::RawSensorT;
+	using RawVectorT = typename Consts::RawVectorT;
 
 	RuntimeCalibrator(
 		SensorFusion& fusion,
 		IMU& imu,
 		uint8_t sensorId,
 		Logging::Logger& logger,
-		float TempTs,
-		float AScale,
-		float GScale,
 		SensorToggleState& toggles
 	)
-		: Base{fusion, imu, sensorId, logger, TempTs, AScale, GScale, toggles} {
-		calibration.T_Ts = TempTs;
-		activeCalibration.T_Ts = TempTs;
+		: Base{fusion, imu, sensorId, logger, toggles} {
+		calibration.T_Ts = Consts::getDefaultTempTs();
+		activeCalibration.T_Ts = Consts::getDefaultTempTs();
 	}
 
 	bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration
@@ -152,22 +152,22 @@ public:
 	}
 
 	void scaleAccelSample(sensor_real_t accelSample[3]) final {
-		accelSample[0] = accelSample[0] * AScale - activeCalibration.A_off[0];
-		accelSample[1] = accelSample[1] * AScale - activeCalibration.A_off[1];
-		accelSample[2] = accelSample[2] * AScale - activeCalibration.A_off[2];
+		accelSample[0] = accelSample[0] * Consts::AScale - activeCalibration.A_off[0];
+		accelSample[1] = accelSample[1] * Consts::AScale - activeCalibration.A_off[1];
+		accelSample[2] = accelSample[2] * Consts::AScale - activeCalibration.A_off[2];
 	}
 
 	float getAccelTimestep() final { return activeCalibration.A_Ts; }
 
 	void scaleGyroSample(sensor_real_t gyroSample[3]) final {
 		gyroSample[0] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[0] - activeCalibration.G_off1[0])
+			Consts::GScale * (gyroSample[0] - activeCalibration.G_off1[0])
 		);
 		gyroSample[1] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[1] - activeCalibration.G_off1[1])
+			Consts::GScale * (gyroSample[1] - activeCalibration.G_off1[1])
 		);
 		gyroSample[2] = static_cast<sensor_real_t>(
-			GScale * (gyroSample[2] - activeCalibration.G_off1[2])
+			Consts::GScale * (gyroSample[2] - activeCalibration.G_off1[2])
 		);
 	}
 
@@ -202,12 +202,12 @@ public:
 			activeZROChange = IMU::TemperatureZROChange;
 		}
 
-		float diffX
-			= (activeCalibration.G_off2[0] - activeCalibration.G_off1[0]) * GScale;
-		float diffY
-			= (activeCalibration.G_off2[1] - activeCalibration.G_off1[1]) * GScale;
-		float diffZ
-			= (activeCalibration.G_off2[2] - activeCalibration.G_off1[2]) * GScale;
+		float diffX = (activeCalibration.G_off2[0] - activeCalibration.G_off1[0])
+					* Consts::GScale;
+		float diffY = (activeCalibration.G_off2[1] - activeCalibration.G_off1[1])
+					* Consts::GScale;
+		float diffZ = (activeCalibration.G_off2[2] - activeCalibration.G_off1[2])
+					* Consts::GScale;
 
 		float maxDiff
 			= std::max(std::max(std::abs(diffX), std::abs(diffY)), std::abs(diffZ));
@@ -402,7 +402,7 @@ private:
 	GyroBiasCalibrationStep<RawSensorT> gyroBiasCalibrationStep{calibration};
 	AccelBiasCalibrationStep<RawSensorT> accelBiasCalibrationStep{
 		calibration,
-		static_cast<float>(Base::AScale)
+		static_cast<float>(Consts::AScale)
 	};
 	NullCalibrationStep<RawSensorT> nullCalibrationStep{calibration};
 
@@ -440,9 +440,7 @@ private:
 
 	Configuration::RuntimeCalibrationSensorConfig activeCalibration = calibration;
 
-	using Base::AScale;
 	using Base::fusion;
-	using Base::GScale;
 	using Base::logger;
 	using Base::sensor;
 	using Base::sensorId;

src/sensors/softfusion/softfusionsensor.h

@@ -23,54 +23,31 @@
 
 #pragma once
 
-#include <cstdint>
-#include <cstdlib>
-#include <tuple>
+#include <PinInterface.h>
 
-#include "../../sensorinterface/i2cimpl.h"
+#include <cstdint>
+#include <cstring>
+
+#include "../../GlobalVars.h"
+#include "../../sensorinterface/SensorInterface.h"
 #include "../RestCalibrationDetector.h"
 #include "../sensor.h"
-#include "GlobalVars.h"
-#include "PinInterface.h"
+#include "TempGradientCalculator.h"
+#include "imuconsts.h"
 #include "motionprocessing/types.h"
 #include "sensors/SensorFusion.h"
-#include "sensors/softfusion/TempGradientCalculator.h"
+#include "sensors/softfusion/magdriver.h"
 
 namespace SlimeVR::Sensors {
 
-template <
-	typename SensorType,
-	template <typename IMU, typename RawSensorT, typename RawVectorT>
-	typename Calibrator>
+template <typename SensorType, template <typename IMU> typename Calibrator>
 class SoftFusionSensor : public Sensor {
-	static constexpr sensor_real_t getDefaultTempTs() {
-		if constexpr (DirectTempReadOnly) {
-			return DirectTempReadTs;
-		} else {
-			return SensorType::TempTs;
-		}
-	}
-
-	static constexpr bool Uses32BitSensorData
-		= requires(SensorType& i) { i.Uses32BitSensorData; };
-
-	static constexpr bool DirectTempReadOnly
-		= requires(SensorType& i) { i.getDirectTemp(); };
-
-	using RawSensorT =
-		typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
-	using RawVectorT = std::array<RawSensorT, 3>;
-
-	static constexpr float GScale
-		= ((32768. / SensorType::GyroSensitivity) / 32768.) * (PI / 180.0);
-	static constexpr float AScale = CONST_EARTH_GRAVITY / SensorType::AccelSensitivity;
-
-	using Calib = Calibrator<SensorType, RawSensorT, RawVectorT>;
+	using Consts = IMUConsts<SensorType>;
+	using RawSensorT = typename Consts::RawSensorT;
 
+	using Calib = Calibrator<SensorType>;
 	static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration;
 
-	static constexpr float DirectTempReadFreq = 15;
-	static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq;
 	float lastReadTemperature = 0;
 	uint32_t lastTempPollTime = micros();
 
@@ -107,7 +84,7 @@ class SoftFusionSensor : public Sensor {
 		}
 	}
 
-	void sendData() {
+	void sendData() final {
 		Sensor::sendData();
 		sendTempIfNeeded();
 	}
@@ -155,7 +132,7 @@ class SoftFusionSensor : public Sensor {
 
 	void
 	processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) {
-		if constexpr (!DirectTempReadOnly) {
+		if constexpr (!Consts::DirectTempReadOnly) {
 			const float scaledTemperature
 				= SensorType::TemperatureBias
 				+ static_cast<float>(rawTemperature)
@@ -173,54 +150,6 @@ class SoftFusionSensor : public Sensor {
 		}
 	}
 
-	void eatSamplesForSeconds(const uint32_t seconds) {
-		const auto targetDelay = millis() + 1000 * seconds;
-		auto lastSecondsRemaining = seconds;
-		while (millis() < targetDelay) {
-#ifdef ESP8266
-			ESP.wdtFeed();
-#endif
-			auto currentSecondsRemaining = (targetDelay - millis()) / 1000;
-			if (currentSecondsRemaining != lastSecondsRemaining) {
-				m_Logger.info("%ld...", currentSecondsRemaining + 1);
-				lastSecondsRemaining = currentSecondsRemaining;
-			}
-			m_sensor.bulkRead(
-				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
-				[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
-				[](const int16_t xyz, const sensor_real_t timeDelta) {}
-			);
-		}
-	}
-
-	std::tuple<RawVectorT, RawVectorT, int16_t> eatSamplesReturnLast(
-		const uint32_t milliseconds
-	) {
-		RawVectorT accel = {0};
-		RawVectorT gyro = {0};
-		int16_t temp = 0;
-		const auto targetDelay = millis() + milliseconds;
-		while (millis() < targetDelay) {
-			m_sensor.bulkRead(
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					accel[0] = xyz[0];
-					accel[1] = xyz[1];
-					accel[2] = xyz[2];
-				},
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					gyro[0] = xyz[0];
-					gyro[1] = xyz[1];
-					gyro[2] = xyz[2];
-				},
-				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
-					temp = rawTemp;
-				}
-			);
-			yield();
-		}
-		return std::make_tuple(accel, gyro, temp);
-	}
-
 public:
 	static constexpr auto TypeID = SensorType::Type;
 	static constexpr uint8_t Address = SensorType::Address;
@@ -276,12 +205,13 @@ public:
 		// read fifo updating fusion
 		uint32_t now = micros();
 
-		if constexpr (DirectTempReadOnly) {
+		if constexpr (Consts::DirectTempReadOnly) {
 			uint32_t tempElapsed = now - lastTempPollTime;
-			if (tempElapsed >= DirectTempReadTs * 1e6) {
+			if (tempElapsed >= Consts::DirectTempReadTs * 1e6) {
 				lastTempPollTime
 					= now
-					- (tempElapsed - static_cast<uint32_t>(DirectTempReadTs * 1e6));
+					- (tempElapsed
+					   - static_cast<uint32_t>(Consts::DirectTempReadTs * 1e6));
 				lastReadTemperature = m_sensor.getDirectTemp();
 
 				calibrator.provideTempSample(lastReadTemperature);
@@ -289,7 +219,7 @@ public:
 				if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) {
 					tempGradientCalculator.feedSample(
 						lastReadTemperature,
-						DirectTempReadTs
+						Consts::DirectTempReadTs
 					);
 				}
 			}
@@ -308,20 +238,20 @@ public:
 		// send new fusion values when time is up
 		now = micros();
 		constexpr float maxSendRateHz = 100.0f;
-		constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6;
+		constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6f;
 		elapsed = now - m_lastRotationPacketSent;
 		if (elapsed >= sendInterval) {
-			m_sensor.bulkRead(
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					processAccelSample(xyz, timeDelta);
+			m_sensor.bulkRead({
+				[&](const auto sample[3], float AccTs) {
+					processAccelSample(sample, AccTs);
 				},
-				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
-					processGyroSample(xyz, timeDelta);
+				[&](const auto sample[3], float GyrTs) {
+					processGyroSample(sample, GyrTs);
 				},
-				[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
-					processTempSample(rawTemp, timeDelta);
-				}
-			);
+				[&](int16_t sample, float TempTs) {
+					processTempSample(sample, TempTs);
+				},
+			});
 			if (!m_fusion.isUpdated()) {
 				checkSensorTimeout();
 				return;
@@ -395,43 +325,46 @@ public:
 
 		m_status = SensorStatus::SENSOR_OK;
 		working = true;
-		[[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000);
-		if constexpr (UpsideDownCalibrationInit) {
-			auto gravity = static_cast<sensor_real_t>(
-				AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
-			);
-			m_Logger.info(
-				"Gravity read: %.1f (need < -7.5 to start calibration)",
-				gravity
-			);
-			if (gravity < -7.5f) {
-				ledManager.on();
-				m_Logger.info("Flip front in 5 seconds to start calibration");
-				lastRawSample = eatSamplesReturnLast(5000);
-				gravity = static_cast<sensor_real_t>(
-					AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
-				);
-				if (gravity > 7.5f) {
-					m_Logger.debug("Starting calibration...");
-					startCalibration(0);
-				} else {
-					m_Logger.info("Flip not detected. Skipping calibration.");
-				}
 
-				ledManager.off();
+		calibrator.checkStartupCalibration();
+
+		if constexpr (Consts::SupportsMags) {
+			magDriver.init(
+				SoftFusion::MagInterface{
+					.readByte
+					= [&](uint8_t address) { return m_sensor.readAux(address); },
+					.writeByte = [&](uint8_t address, uint8_t value) {},
+					.setDeviceId
+					= [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); },
+					.startPolling
+					= [&](uint8_t dataReg, SoftFusion::MagDataWidth dataWidth
+					  ) { m_sensor.startAuxPolling(dataReg, dataWidth); },
+					.stopPolling = [&]() { m_sensor.stopAuxPolling(); },
+				},
+				Consts::Supports9ByteMag
+			);
+
+			if (toggles.getToggle(SensorToggles::MagEnabled)) {
+				magDriver.startPolling();
 			}
 		}
+
+		toggles.onToggleChange([&](SensorToggles toggle, bool value) {
+			if (toggle == SensorToggles::MagEnabled) {
+				if (value) {
+					magDriver.startPolling();
+				} else {
+					magDriver.stopPolling();
+				}
+			}
+		});
 	}
 
 	void startCalibration(int calibrationType) final {
-		calibrator.startCalibration(
-			calibrationType,
-			[&](const uint32_t seconds) { eatSamplesForSeconds(seconds); },
-			[&](const uint32_t millis) { return eatSamplesReturnLast(millis); }
-		);
+		calibrator.startCalibration(calibrationType);
 	}
 
-	bool isFlagSupported(SensorToggles toggle) const final {
+	[[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final {
 		return toggle == SensorToggles::CalibrationEnabled
 			|| toggle == SensorToggles::TempGradientCalibrationEnabled;
 	}
@@ -440,16 +373,7 @@ public:
 
 	SensorFusion m_fusion;
 	SensorType m_sensor;
-	Calib calibrator{
-		m_fusion,
-		m_sensor,
-		sensorId,
-		m_Logger,
-		getDefaultTempTs(),
-		AScale,
-		GScale,
-		toggles
-	};
+	Calib calibrator{m_fusion, m_sensor, sensorId, m_Logger, toggles};
 
 	SensorStatus m_status = SensorStatus::SENSOR_OFFLINE;
 	uint32_t m_lastPollTime = micros();
@@ -459,6 +383,8 @@ public:
 
 	RestCalibrationDetector calibrationDetector;
 
+	SoftFusion::MagDriver magDriver;
+
 	static bool checkPresent(const RegisterInterface& imuInterface) {
 		I2Cdev::readTimeout = 100;
 		auto value = imuInterface.readReg(SensorType::Regs::WhoAmI::reg);
@@ -477,6 +403,10 @@ public:
 			return false;
 		}
 	}
+
+	const char* getAttachedMagnetometer() const final {
+		return magDriver.getAttachedMagName();
+	}
 };
 
 }  // namespace SlimeVR::Sensors

src/serial/serialcommands.cpp

@@ -164,6 +164,10 @@ void printState() {
 			sensor->isWorking() ? "true" : "false",
 			sensor->getHadData() ? "true" : "false"
 		);
+		const char* mag = sensor->getAttachedMagnetometer();
+		if (mag) {
+			logger.info("Sensor[%d] magnetometer: %s", sensor->getSensorId(), mag);
+		}
 	}
 	logger.info(
 		"Battery voltage: %.3f, level: %.1f%%",
@@ -288,6 +292,12 @@ void cmdGet(CmdParser* parser) {
 			sensor0->isWorking() ? "true" : "false",
 			sensor0->getHadData() ? "true" : "false"
 		);
+
+		const char* mag = sensor0->getAttachedMagnetometer();
+		if (mag) {
+			logger.info("[TEST] Sensor[0] magnetometer: %s", mag);
+		}
+
 		if (!sensor0->getHadData()) {
 			logger.error("[TEST] Sensor[0] didn't send any data yet!");
 		} else {