Apply some ICM code fixes

Fix: ES32 -> ESP32

lib/math/quat.cpp

@@ -229,3 +229,12 @@ void Quat::set_axis_angle(const Vector3& axis, const float& angle) {
 			cos_angle);
 	}
 }
+
+void Quat::sandwitch(Vector3& v) {
+	float tempX, tempY;
+	tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x;
+	tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x * v.y;
+	v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x - y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z;
+	v.x = tempX;
+	v.y = tempY;
+}

lib/math/quat.h

@@ -79,6 +79,8 @@ public:
 	Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq, const float& t) const;
 	bool equalsWithEpsilon(const Quat& q2);
 
+	void sandwitch(Vector3& vector);
+
 	void set_axis_angle(const Vector3& axis, const float& angle);
 	inline void get_axis_angle(Vector3& r_axis, double& r_angle) const {
 		r_angle = 2 * std::acos(w);

lib/vqf/vqf.cpp

@@ -2,7 +2,7 @@
 //
 // SPDX-License-Identifier: MIT
 
-// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
 // Removed batch update functions
 
 #include "vqf.h"
@@ -38,7 +38,7 @@ VQF::VQF(const VQFParams &params, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t
     setup();
 }
 
-void VQF::updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
+void VQF::updateGyr(const vqf_real_t gyr[3], double gyrTs)
 {
     // rest detection
     if (params.restBiasEstEnabled || params.magDistRejectionEnabled) {
@@ -502,8 +502,8 @@ void VQF::setTauAcc(vqf_real_t tauAcc)
         return;
     }
     params.tauAcc = tauAcc;
-    vqf_real_t newB[3];
-    vqf_real_t newA[3];
+    double newB[3];
+    double newA[3];
 
     filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA);
     filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState);
@@ -696,15 +696,15 @@ vqf_real_t VQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts)
     }
 }
 
-void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[], vqf_real_t outA[])
+void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA[])
 {
     assert(tau > 0);
     assert(Ts > 0);
     // second order Butterworth filter based on https://stackoverflow.com/a/52764064
-    vqf_real_t fc = (M_SQRT2 / (2.0*M_PI))/vqf_real_t(tau); // time constant of dampened, non-oscillating part of step response
-    vqf_real_t C = tan(M_PI*fc*vqf_real_t(Ts));
-    vqf_real_t D = C*C + sqrt(2)*C + 1;
-    vqf_real_t b0 = C*C/D;
+    double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response
+    double C = tan(M_PI*fc*double(Ts));
+    double D = C*C + sqrt(2)*C + 1;
+    double b0 = C*C/D;
     outB[0] = b0;
     outB[1] = 2*b0;
     outB[2] = b0;
@@ -713,7 +713,7 @@ void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[], vqf_rea
     outA[1] = (1-sqrt(2)*C+C*C)/D; // a2
 }
 
-void VQF::filterInitialState(vqf_real_t x0, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t out[])
+void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[])
 {
     // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter
     // update equation)
@@ -721,9 +721,9 @@ void VQF::filterInitialState(vqf_real_t x0, const vqf_real_t b[3], const vqf_rea
     out[1] = x0*(b[2] - a[1]);
 }
 
-void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const vqf_real_t b_old[],
-                                         const vqf_real_t a_old[], const vqf_real_t b_new[],
-                                         const vqf_real_t a_new[], vqf_real_t state[])
+void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[],
+                                         const double a_old[], const double b_new[],
+                                         const double a_new[], double state[])
 {
     if (isnan(state[0])) {
         return;
@@ -734,18 +734,18 @@ void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const vq
     }
 }
 
-vqf_real_t VQF::filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2])
+vqf_real_t VQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2])
 {
     // difference equations based on scipy.signal.lfilter documentation
     // assumes that a0 == 1.0
-    vqf_real_t y = b[0]*x + state[0];
+    double y = b[0]*x + state[0];
     state[0] = b[1]*x - a[0]*y + state[1];
     state[1] = b[2]*x - a[1]*y;
     return y;
 }
 
-void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const vqf_real_t b[3],
-                    const vqf_real_t a[2], vqf_real_t state[], vqf_real_t out[])
+void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3],
+                    const double a[2], double state[], vqf_real_t out[])
 {
     assert(N>=2);
 
@@ -838,17 +838,17 @@ void VQF::matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2
 bool VQF::matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9])
 {
     // in = [a b c; d e f; g h i]
-    vqf_real_t A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
-    vqf_real_t D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
-    vqf_real_t G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
-    vqf_real_t B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
-    vqf_real_t E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
-    vqf_real_t H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
-    vqf_real_t C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
-    vqf_real_t F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
-    vqf_real_t I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
+    double A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
+    double D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
+    double G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
+    double B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
+    double E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
+    double H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
+    double C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
+    double F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
+    double I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
 
-    vqf_real_t det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
+    double det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
 
     if (det >= -EPS && det <= EPS) {
         std::fill(out, out+9, 0);
@@ -909,4 +909,4 @@ void VQF::updateBiasForgettingTime(float biasForgettingTime) {
 
     vqf_real_t pRest = square(params.biasSigmaRest*100.0);
     coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
-}
+}

lib/vqf/vqf.h

@@ -2,7 +2,7 @@
 //
 // SPDX-License-Identifier: MIT
 
-// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
+// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
 // Removed batch update functions
 
 #ifndef VQF_HPP
@@ -17,9 +17,9 @@
 /**
  * @brief Typedef for the floating-point data type used for most operations.
  *
- * By default, all floating-point calculations are performed using `vqf_real_t`. Set the
+ * By default, all floating-point calculations are performed using `double`. Set the
  * `VQF_SINGLE_PRECISION` define to change this type to `float`. Note that the
- * Butterworth filter implementation will always use vqf_real_t precision as using floats
+ * Butterworth filter implementation will always use double precision as using floats
  * can cause numeric issues.
  */
 #ifndef VQF_SINGLE_PRECISION
@@ -355,7 +355,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for #lastAccLp.
 	 */
-	vqf_real_t accLpState[3 * 2];
+	double accLpState[3 * 2];
 	/**
 	 * @brief Last inclination correction angular rate.
 	 *
@@ -413,12 +413,12 @@ struct VQFState {
 	 * @brief Internal state of the Butterworth low-pass filter for the rotation matrix
 	 * coefficients used in motion bias estimation.
 	 */
-	vqf_real_t motionBiasEstRLpState[9 * 2];
+	double motionBiasEstRLpState[9 * 2];
 	/**
 	 * @brief Internal low-pass filter state for the rotated bias estimate used in
 	 * motion bias estimation.
 	 */
-	vqf_real_t motionBiasEstBiasLpState[2 * 2];
+	double motionBiasEstBiasLpState[2 * 2];
 #endif
 	/**
 	 * @brief Last (squared) deviations from the reference of the last sample used in
@@ -451,7 +451,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for #restLastGyrLp.
 	 */
-	vqf_real_t restGyrLpState[3 * 2];
+	double restGyrLpState[3 * 2];
 	/**
 	 * @brief Last low-pass filtered accelerometer measurement used as the reference for
 	 * rest detection.
@@ -460,7 +460,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for #restLastAccLp.
 	 */
-	vqf_real_t restAccLpState[3 * 2];
+	double restAccLpState[3 * 2];
 
 	/**
 	 * @brief Norm of the currently accepted magnetic field reference.
@@ -514,7 +514,7 @@ struct VQFState {
 	/**
 	 * @brief Internal low-pass filter state for the current norm and dip angle.
 	 */
-	vqf_real_t magNormDipLpState[2 * 2];
+	double magNormDipLpState[2 * 2];
 };
 
 /**
@@ -542,13 +542,13 @@ struct VQFCoefficients {
 	 *
 	 * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$.
 	 */
-	vqf_real_t accLpB[3];
+	double accLpB[3];
 	/**
 	 * @brief Denominator coefficients of the acceleration low-pass filter.
 	 *
 	 * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$.
 	 */
-	vqf_real_t accLpA[2];
+	double accLpA[2];
 
 	/**
 	 * @brief Gain of the first-order filter used for heading correction.
@@ -584,22 +584,22 @@ struct VQFCoefficients {
 	 * @brief Numerator coefficients of the gyroscope measurement low-pass filter for
 	 * rest detection.
 	 */
-	vqf_real_t restGyrLpB[3];
+	double restGyrLpB[3];
 	/**
 	 * @brief Denominator coefficients of the gyroscope measurement low-pass filter for
 	 * rest detection.
 	 */
-	vqf_real_t restGyrLpA[2];
+	double restGyrLpA[2];
 	/**
 	 * @brief Numerator coefficients of the accelerometer measurement low-pass filter
 	 * for rest detection.
 	 */
-	vqf_real_t restAccLpB[3];
+	double restAccLpB[3];
 	/**
 	 * @brief Denominator coefficients of the accelerometer measurement low-pass filter
 	 * for rest detection.
 	 */
-	vqf_real_t restAccLpA[2];
+	double restAccLpA[2];
 
 	/**
 	 * @brief Gain of the first-order filter used for to update the magnetic field
@@ -610,12 +610,12 @@ struct VQFCoefficients {
 	 * @brief Numerator coefficients of the low-pass filter for the current magnetic
 	 * norm and dip.
 	 */
-	vqf_real_t magNormDipLpB[3];
+	double magNormDipLpB[3];
 	/**
 	 * @brief Denominator coefficients of the low-pass filter for the current magnetic
 	 * norm and dip.
 	 */
-	vqf_real_t magNormDipLpA[2];
+	double magNormDipLpA[2];
 };
 
 /**
@@ -723,7 +723,7 @@ public:
 	 *
 	 * @param gyr gyroscope measurement in rad/s
 	 */
-	void updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs);
+	void updateGyr(const vqf_real_t gyr[3], double gyrTs);
 	/**
 	 * @brief Performs accelerometer update step.
 	 *
@@ -979,7 +979,7 @@ public:
 	 * @param outA output array for denominator coefficients (without \f$a_0=1\f$)
 	 */
 	static void
-	filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[3], vqf_real_t outA[2]);
+	filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]);
 	/**
 	 * @brief Calculates the initial filter state for a given steady-state value.
 	 * @param x0 steady state value
@@ -989,9 +989,9 @@ public:
 	 */
 	static void filterInitialState(
 		vqf_real_t x0,
-		const vqf_real_t b[],
-		const vqf_real_t a[],
-		vqf_real_t out[2]
+		const double b[],
+		const double a[],
+		double out[2]
 	);
 	/**
 	 * @brief Adjusts the filter state when changing coefficients.
@@ -1012,11 +1012,11 @@ public:
 	static void filterAdaptStateForCoeffChange(
 		vqf_real_t last_y[],
 		size_t N,
-		const vqf_real_t b_old[3],
-		const vqf_real_t a_old[2],
-		const vqf_real_t b_new[3],
-		const vqf_real_t a_new[2],
-		vqf_real_t state[]
+		const double b_old[3],
+		const double a_old[2],
+		const double b_new[3],
+		const double a_new[2],
+		double state[]
 	);
 	/**
 	 * @brief Performs a filter step for a scalar value.
@@ -1027,7 +1027,7 @@ public:
 	 * @return filtered value
 	 */
 	static vqf_real_t
-	filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2]);
+	filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]);
 	/**
 	 * @brief Performs filter step for vector-valued signal with averaging-based
 	 * initialization.
@@ -1052,9 +1052,9 @@ public:
 		size_t N,
 		vqf_real_t tau,
 		vqf_real_t Ts,
-		const vqf_real_t b[3],
-		const vqf_real_t a[2],
-		vqf_real_t state[],
+		const double b[3],
+		const double a[2],
+		double state[],
 		vqf_real_t out[]
 	);
 #ifndef VQF_NO_MOTION_BIAS_ESTIMATION

platformio-tools.ini

@@ -72,7 +72,7 @@ build_flags =
   -DESP32C3
 board = dfrobot_beetle_esp32c3
 
-[env:BOARD_ES32C3DEVKITM1]
+[env:BOARD_ESP32C3DEVKITM1]
 platform = espressif32 @ 6.7.0
 platform_packages =
   framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1
@@ -82,7 +82,7 @@ build_flags =
   -DESP32C3
 board = esp32-c3-devkitm-1
 
-[env:BOARD_ES32C6DEVKITC1]
+[env:BOARD_ESP32C6DEVKITC1]
 platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip
 build_flags =
   ${env.build_flags}

src/consts.h

@@ -49,6 +49,7 @@ enum class SensorTypeID : uint8_t {
 	Empty = 255
 };
 
+#define IMU_AUTO SensorAuto
 #define IMU_UNKNOWN ErroneousSensor
 #define IMU_MPU9250 MPU9250Sensor
 #define IMU_MPU6500 MPU6050Sensor
@@ -83,14 +84,14 @@ enum class SensorTypeID : uint8_t {
 #define BOARD_SLIMEVR 9
 #define BOARD_LOLIN_C3_MINI 10
 #define BOARD_BEETLE32C3 11
-#define BOARD_ES32C3DEVKITM1 12
+#define BOARD_ESP32C3DEVKITM1 12
 #define BOARD_OWOTRACK 13  // Only used by owoTrack mobile app
 #define BOARD_WRANGLER 14  // Only used by wrangler app
 #define BOARD_MOCOPI 15  // Used by mocopi/moslime
 #define BOARD_WEMOSWROOM02 16
 #define BOARD_XIAO_ESP32C3 17
 #define BOARD_HARITORA 18  // Used by Haritora/SlimeTora
-#define BOARD_ES32C6DEVKITC1 19
+#define BOARD_ESP32C6DEVKITC1 19
 #define BOARD_GLOVE_IMU_SLIMEVR_DEV 20  // IMU Glove
 #define BOARD_GESTURES 21  // Used by Gestures
 #define BOARD_DEV_RESERVED 250  // Reserved, should not be used in any release firmware

src/debug.h

@@ -58,7 +58,7 @@
 #define samplingRateInMillis 10
 
 // Sleeping options
-#define POWERSAVING_MODE POWER_SAVING_LEGACY  // Minimum causes sporadic data pauses
+#define POWERSAVING_MODE POWER_SAVING_NONE
 #if POWERSAVING_MODE >= POWER_SAVING_MINIMUM
 #define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000)
 #endif

src/defines.h

@@ -26,8 +26,8 @@
 // ================================================
 
 // Set parameters of IMU and board used
-#define IMU IMU_BNO085
-#define SECOND_IMU IMU
+#define IMU IMU_AUTO
+#define SECOND_IMU IMU_AUTO
 #define BOARD BOARD_SLIMEVR
 #define IMU_ROTATION DEG_270
 #define SECOND_IMU_ROTATION DEG_270
@@ -54,24 +54,24 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
 */
 
 #ifndef SENSOR_DESC_LIST
-#define SENSOR_DESC_LIST                       \
-	SENSOR_DESC_ENTRY(                         \
-		IMU,                                   \
-		PRIMARY_IMU_ADDRESS_ONE,               \
-		IMU_ROTATION,                          \
-		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \
-		PRIMARY_IMU_OPTIONAL,                  \
-		DIRECT_PIN(PIN_IMU_INT),               \
-		0                                      \
-	)                                          \
-	SENSOR_DESC_ENTRY(                         \
-		SECOND_IMU,                            \
-		SECONDARY_IMU_ADDRESS_TWO,             \
-		SECOND_IMU_ROTATION,                   \
-		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \
-		SECONDARY_IMU_OPTIONAL,                \
-		DIRECT_PIN(PIN_IMU_INT_2),             \
-		0                                      \
+#define SENSOR_DESC_LIST                                             \
+	SENSOR_DESC_ENTRY(                                           \
+		IMU,                                                         \
+		DIRECT_SPI(24'000'000, MSBFIRST, SPI_MODE3, DIRECT_PIN(15)), \
+		IMU_ROTATION,                                                \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA),                       \
+		PRIMARY_IMU_OPTIONAL,                                        \
+		DIRECT_PIN(PIN_IMU_INT),                                     \
+		0                                                            \
+	)                                                                \
+	SENSOR_DESC_ENTRY(                                               \
+		SECOND_IMU,                                                  \
+		SECONDARY_IMU_ADDRESS_TWO,                                   \
+		SECOND_IMU_ROTATION,                                         \
+		DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA),                       \
+		SECONDARY_IMU_OPTIONAL,                                      \
+		DIRECT_PIN(PIN_IMU_INT_2),                                   \
+		0                                                            \
 	)
 #endif
 
@@ -239,10 +239,10 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
 
 // Board-specific configurations
 #if BOARD == BOARD_SLIMEVR
-#define PIN_IMU_SDA 14
-#define PIN_IMU_SCL 12
-#define PIN_IMU_INT 16
-#define PIN_IMU_INT_2 13
+#define PIN_IMU_SDA 4
+#define PIN_IMU_SCL 5
+#define PIN_IMU_INT 2
+#define PIN_IMU_INT_2 16
 #define PIN_BATTERY_LEVEL 17
 #define LED_PIN 2
 #define LED_INVERTED true
@@ -331,7 +331,7 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
 #define PIN_BATTERY_LEVEL 3
 #define LED_PIN 10
 #define LED_INVERTED false
-#elif BOARD == BOARD_ES32C3DEVKITM1 || BOARD == BOARD_ES32C6DEVKITC1
+#elif BOARD == BOARD_ESP32C3DEVKITM1 || BOARD == BOARD_ESP32C6DEVKITC1
 #define PIN_IMU_SDA 5
 #define PIN_IMU_SCL 4
 #define PIN_IMU_INT 6

src/logging/Logger.cpp

@@ -7,49 +7,49 @@ void Logger::setTag(const char* tag) {
 	strcpy(m_Tag, tag);
 }
 
-void Logger::trace(const char* format, ...) {
+void Logger::trace(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(TRACE, format, args);
 	va_end(args);
 }
 
-void Logger::debug(const char* format, ...) {
+void Logger::debug(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(DEBUG, format, args);
 	va_end(args);
 }
 
-void Logger::info(const char* format, ...) {
+void Logger::info(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(INFO, format, args);
 	va_end(args);
 }
 
-void Logger::warn(const char* format, ...) {
+void Logger::warn(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(WARN, format, args);
 	va_end(args);
 }
 
-void Logger::error(const char* format, ...) {
+void Logger::error(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(ERROR, format, args);
 	va_end(args);
 }
 
-void Logger::fatal(const char* format, ...) {
+void Logger::fatal(const char* format, ...) const {
 	va_list args;
 	va_start(args, format);
 	log(FATAL, format, args);
 	va_end(args);
 }
 
-void Logger::log(Level level, const char* format, va_list args) {
+void Logger::log(Level level, const char* format, va_list args) const {
 	if (level < LOG_LEVEL) {
 		return;
 	}

src/logging/Logger.h

@@ -27,48 +27,48 @@ public:
 
 	void setTag(const char* tag);
 
-	void trace(const char* str, ...);
-	void debug(const char* str, ...);
-	void info(const char* str, ...);
-	void warn(const char* str, ...);
-	void error(const char* str, ...);
-	void fatal(const char* str, ...);
+	void trace(const char* str, ...) const;
+	void debug(const char* str, ...) const;
+	void info(const char* str, ...) const;
+	void warn(const char* str, ...) const;
+	void error(const char* str, ...) const;
+	void fatal(const char* str, ...) const;
 
 	template <typename T>
-	inline void traceArray(const char* str, const T* array, size_t size) {
+	inline void traceArray(const char* str, const T* array, size_t size) const {
 		logArray(TRACE, str, array, size);
 	}
 
 	template <typename T>
-	inline void debugArray(const char* str, const T* array, size_t size) {
+	inline void debugArray(const char* str, const T* array, size_t size) const {
 		logArray(DEBUG, str, array, size);
 	}
 
 	template <typename T>
-	inline void infoArray(const char* str, const T* array, size_t size) {
+	inline void infoArray(const char* str, const T* array, size_t size) const {
 		logArray(INFO, str, array, size);
 	}
 
 	template <typename T>
-	inline void warnArray(const char* str, const T* array, size_t size) {
+	inline void warnArray(const char* str, const T* array, size_t size) const {
 		logArray(WARN, str, array, size);
 	}
 
 	template <typename T>
-	inline void errorArray(const char* str, const T* array, size_t size) {
+	inline void errorArray(const char* str, const T* array, size_t size) const {
 		logArray(ERROR, str, array, size);
 	}
 
 	template <typename T>
-	inline void fatalArray(const char* str, const T* array, size_t size) {
+	inline void fatalArray(const char* str, const T* array, size_t size) const {
 		logArray(FATAL, str, array, size);
 	}
 
 private:
-	void log(Level level, const char* str, va_list args);
+	void log(Level level, const char* str, va_list args) const;
 
 	template <typename T>
-	void logArray(Level level, const char* str, const T* array, size_t size) {
+	void logArray(Level level, const char* str, const T* array, size_t size) const {
 		if (level < LOG_LEVEL) {
 			return;
 		}

src/main.cpp

@@ -30,6 +30,7 @@
 #include "debugging/TimeTaken.h"
 #include "globals.h"
 #include "logging/Logger.h"
+#include "onOffButton.h"
 #include "ota.h"
 #include "serial/serialcommands.h"
 #include "status/TPSCounter.h"
@@ -47,6 +48,10 @@ SlimeVR::Network::Connection networkConnection;
 SlimeVR::Debugging::TimeTakenMeasurer sensorMeasurer{"Sensors"};
 #endif
 
+#ifdef ON_OFF_BUTTON
+SlimeVR::OnOffButton onOffButton;
+#endif
+
 int sensorToCalibrate = -1;
 bool blinking = false;
 unsigned long blinkStart = 0;
@@ -78,7 +83,7 @@ void setup() {
 	// this, check needs to be re-added.
 	auto clearResult = I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL);
 	if (clearResult != 0) {
-		logger.error("Can't clear I2C bus, error %d", clearResult);
+		logger.warn("Can't clear I2C bus, error %d", clearResult);
 	}
 
 	// join I2C bus
@@ -101,6 +106,10 @@ void setup() {
 #endif
 	Wire.setClock(I2C_SPEED);
 
+#ifdef ON_OFF_BUTTON
+	onOffButton.setup();
+#endif
+
 	// Wait for IMU to boot
 	delay(500);
 
@@ -136,6 +145,11 @@ void loop() {
 	battery.Loop();
 	ledManager.update();
 	I2CSCAN::update();
+
+#ifdef ON_OFF_BUTTON
+	onOffButton.update();
+#endif
+
 #ifdef TARGET_LOOPTIME_MICROS
 	long elapsed = (micros() - loopTime);
 	if (elapsed < TARGET_LOOPTIME_MICROS) {

src/network/connection.h

@@ -137,9 +137,9 @@ public:
 	bool endBundle();
 
 private:
-	void updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors);
+	void updateSensorState(std::vector<std::unique_ptr<::Sensor>>& sensors);
 	void maybeRequestFeatureFlags();
-	bool isSensorStateUpdated(int i, std::unique_ptr<Sensor>& sensor);
+	bool isSensorStateUpdated(int i, std::unique_ptr<::Sensor>& sensor);
 
 	bool beginPacket();
 	bool endPacket();
@@ -209,7 +209,7 @@ private:
 	void sendTrackerDiscovery();
 
 	// PACKET_SENSOR_INFO 15
-	void sendSensorInfo(Sensor& sensor);
+	void sendSensorInfo(::Sensor& sensor);
 
 	void sendAcknowledgeConfigChange(uint8_t sensorId, SensorToggles configType);
 

src/onOffButton.cpp

@@ -0,0 +1,77 @@
+#include "onOffButton.h"
+
+#include "GlobalVars.h"
+
+void SlimeVR::OnOffButton::setup() {
+#ifdef ON_OFF_BUTTON
+
+#ifdef ESP8266
+	digitalWrite(D0, LOW);
+	pinMode(D0, OUTPUT);
+	pinMode(ON_OFF_BUTTON, INPUT);
+#endif
+
+#ifdef ESP32
+	pinMode(
+		ON_OFF_BUTTON,
+		ON_OFF_BUTTON_ACTIVE_LEVEL == LOW ? INPUT_PULLUP : INPUT_PULLDOWN
+	);
+	esp_deep_sleep_enable_gpio_wakeup(
+		1 << ON_OFF_BUTTON,
+		ON_OFF_BUTTON_ACTIVE_LEVEL == LOW ? ESP_GPIO_WAKEUP_GPIO_LOW
+										  : ESP_GPIO_WAKEUP_GPIO_HIGH
+	);
+#endif
+
+#endif
+}
+
+void SlimeVR::OnOffButton::update() {
+#ifdef ON_OFF_BUTTON
+
+	if (digitalRead(ON_OFF_BUTTON) != ON_OFF_BUTTON_ACTIVE_LEVEL) {
+		return;
+	}
+
+	uint32_t ringBuffer = 0;
+	long startTime = millis();
+	while (millis() - startTime < ON_OFF_BUTTON_HOLD_TIME_MS) {
+		ringBuffer <<= 1;
+		ringBuffer |= digitalRead(ON_OFF_BUTTON) != ON_OFF_BUTTON_ACTIVE_LEVEL;
+
+		int popCount = __builtin_popcount(ringBuffer);
+		if (popCount > 16) {
+			return;
+		}
+		delay(1);
+	}
+
+	ledManager.off();
+	for (int i = 0; i < 3; i++) {
+		ledManager.on();
+		delay(100);
+		ledManager.off();
+		delay(100);
+	}
+
+	ringBuffer = 0;
+	while (__builtin_popcount(ringBuffer) <= 16) {
+		ringBuffer <<= 1;
+		ringBuffer |= digitalRead(ON_OFF_BUTTON) != ON_OFF_BUTTON_ACTIVE_LEVEL;
+		delay(1);
+	}
+
+	const auto& sensors = sensorManager.getSensors();
+	for (auto& sensor : sensors) {
+		sensor->deinitialize();
+	}
+
+#ifdef ESP8266
+	ESP.deepSleep(0);
+#endif
+
+#ifdef ESP32
+	esp_deep_sleep_start();
+#endif
+#endif
+}

src/onOffButton.h

@@ -0,0 +1,24 @@
+#ifndef SLIMEVR_ONOFFBUTTON_H_
+#define SLIMEVR_ONOFFBUTTON_H_
+
+#include "globals.h"
+#include "logging/Logger.h"
+
+#ifndef ON_OFF_BUTTON_ACTIVE_LEVEL
+#define ON_OFF_BUTTON_ACTIVE_LEVEL LOW
+#endif
+
+namespace SlimeVR {
+
+class OnOffButton {
+public:
+	void setup();
+	void update();
+
+private:
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("OnOffButton");
+};
+
+}  // namespace SlimeVR
+
+#endif

src/sensorinterface/RegisterInterface.h

@@ -0,0 +1,45 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Tailsy13 & 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 "I2Cdev.h"
+
+namespace SlimeVR::Sensors {
+
+struct RegisterInterface {
+	static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2;
+
+	virtual uint8_t readReg(uint8_t regAddr) const = 0;
+	virtual uint16_t readReg16(uint8_t regAddr) const = 0;
+	virtual void writeReg(uint8_t regAddr, uint8_t value) const = 0;
+	virtual void writeReg16(uint8_t regAddr, uint16_t value) const = 0;
+	virtual void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0;
+	virtual void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0;
+	virtual uint8_t getAddress() const = 0;
+	virtual bool hasSensorOnBus() = 0;
+	virtual std::string toString() const = 0;
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion

src/sensorinterface/SPIImpl.h

@@ -0,0 +1,151 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Eiren Rain & 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 <PinInterface.h>
+#include <SPI.h>
+
+#include <cstdint>
+
+#include "../logging/Logger.h"
+#include "RegisterInterface.h"
+
+#define ICM_READ_FLAG 0x80
+
+namespace SlimeVR::Sensors {
+
+struct SPIImpl : public RegisterInterface {
+	SPIImpl(uint8_t address)
+		: m_spiClass(SPI)
+		, m_spiSettings(SPISettings())
+		, m_csPin(nullptr) {
+		static_assert("SPI requires explicit declaration");
+	}
+
+	SPIImpl(SPIClass& spiClass, SPISettings spiSettings, PinInterface* csPin)
+		: m_spiClass(spiClass)
+		, m_spiSettings(spiSettings)
+		, m_csPin(csPin) {
+		m_Logger.info(
+			"SPI settings: clock: %d, bit order: 0x%02X, data mode: 0x%02X",
+			spiSettings._clock,
+			spiSettings._bitOrder,
+			spiSettings._dataMode
+		);
+		csPin->pinMode(OUTPUT);
+		csPin->digitalWrite(HIGH);
+		spiClass.begin();
+	}
+
+	uint8_t readReg(uint8_t regAddr) const override {
+		m_spiClass.beginTransaction(m_spiSettings);
+		m_csPin->digitalWrite(LOW);
+
+		m_spiClass.transfer(regAddr | ICM_READ_FLAG);
+		uint8_t buffer = m_spiClass.transfer(0);
+
+		m_csPin->digitalWrite(HIGH);
+		m_spiClass.endTransaction();
+
+		return buffer;
+	}
+
+	uint16_t readReg16(uint8_t regAddr) const override {
+		m_spiClass.beginTransaction(m_spiSettings);
+		m_csPin->digitalWrite(LOW);
+
+		m_spiClass.transfer(regAddr | ICM_READ_FLAG);
+		uint8_t b1 = m_spiClass.transfer(0);
+		uint8_t b2 = m_spiClass.transfer(0);
+
+		m_csPin->digitalWrite(HIGH);
+		m_spiClass.endTransaction();
+		return b2 << 8 | b1;
+	}
+
+	void writeReg(uint8_t regAddr, uint8_t value) const override {
+		m_spiClass.beginTransaction(m_spiSettings);
+		m_csPin->digitalWrite(LOW);
+
+		m_spiClass.transfer(regAddr);
+		m_spiClass.transfer(value);
+
+		m_csPin->digitalWrite(HIGH);
+		m_spiClass.endTransaction();
+	}
+
+	void writeReg16(uint8_t regAddr, uint16_t value) const override {
+		m_spiClass.beginTransaction(m_spiSettings);
+		m_csPin->digitalWrite(LOW);
+
+		m_spiClass.transfer(regAddr);
+		m_spiClass.transfer(value & 0xFF);
+		m_spiClass.transfer(value >> 8);
+
+		m_csPin->digitalWrite(HIGH);
+		m_spiClass.endTransaction();
+	}
+
+	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
+		m_spiClass.beginTransaction(m_spiSettings);
+		m_csPin->digitalWrite(LOW);
+		;
+
+		m_spiClass.transfer(regAddr | ICM_READ_FLAG);
+		for (uint8_t i = 0; i < size; ++i) {
+			buffer[i] = m_spiClass.transfer(0);
+		}
+
+		m_csPin->digitalWrite(HIGH);
+		m_spiClass.endTransaction();
+	}
+
+	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
+		m_spiClass.beginTransaction(m_spiSettings);
+		m_csPin->digitalWrite(LOW);
+
+		m_spiClass.transfer(regAddr);
+		for (uint8_t i = 0; i < size; ++i) {
+			m_spiClass.transfer(buffer[i]);
+		}
+
+		m_csPin->digitalWrite(HIGH);
+		m_spiClass.endTransaction();
+	}
+
+	bool hasSensorOnBus() {
+		return true;  // TODO
+	}
+
+	uint8_t getAddress() const override { return 0; }
+
+	std::string toString() const { return std::string("SPI"); }
+
+private:
+	SPIClass& m_spiClass;
+	SPISettings m_spiSettings;
+	PinInterface* m_csPin;
+	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("SPI");
+};
+
+}  // namespace SlimeVR::Sensors

src/sensorinterface/i2cimpl.h

@@ -23,25 +23,26 @@
 
 #pragma once
 
+#include <i2cscan.h>
+
 #include <cstdint>
 
 #include "I2Cdev.h"
+#include "RegisterInterface.h"
 
-namespace SlimeVR::Sensors::SoftFusion {
-
-struct I2CImpl {
-	static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2;
+namespace SlimeVR::Sensors {
 
+struct I2CImpl : public RegisterInterface {
 	I2CImpl(uint8_t devAddr)
 		: m_devAddr(devAddr) {}
 
-	uint8_t readReg(uint8_t regAddr) const {
+	uint8_t readReg(uint8_t regAddr) const override {
 		uint8_t buffer = 0;
 		I2Cdev::readByte(m_devAddr, regAddr, &buffer);
 		return buffer;
 	}
 
-	uint16_t readReg16(uint8_t regAddr) const {
+	uint16_t readReg16(uint8_t regAddr) const override {
 		uint16_t buffer = 0;
 		I2Cdev::readBytes(
 			m_devAddr,
@@ -52,11 +53,11 @@ struct I2CImpl {
 		return buffer;
 	}
 
-	void writeReg(uint8_t regAddr, uint8_t value) const {
+	void writeReg(uint8_t regAddr, uint8_t value) const override {
 		I2Cdev::writeByte(m_devAddr, regAddr, value);
 	}
 
-	void writeReg16(uint8_t regAddr, uint16_t value) const {
+	void writeReg16(uint8_t regAddr, uint16_t value) const override {
 		I2Cdev::writeBytes(
 			m_devAddr,
 			regAddr,
@@ -65,16 +66,24 @@ struct I2CImpl {
 		);
 	}
 
-	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const {
+	void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
 		I2Cdev::readBytes(m_devAddr, regAddr, size, buffer);
 	}
 
-	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const {
+	void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override {
 		I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer);
 	}
 
+	bool hasSensorOnBus() { return I2CSCAN::hasDevOnBus(m_devAddr); }
+
+	uint8_t getAddress() const override { return m_devAddr; }
+
+	std::string toString() const {
+		return std::string("I2C(") + std::to_string(m_devAddr) + std::string(")");
+	}
+
 private:
 	uint8_t m_devAddr;
 };
 
-}  // namespace SlimeVR::Sensors::SoftFusion
+}  // namespace SlimeVR::Sensors

src/sensors/ADCResistanceSensor.cpp

@@ -24,6 +24,8 @@
 
 #include "GlobalVars.h"
 
+namespace SlimeVR {
+namespace Sensors {
 void ADCResistanceSensor::motionLoop() {
 #if ESP8266
 	float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution;
@@ -39,3 +41,6 @@ void ADCResistanceSensor::motionLoop() {
 void ADCResistanceSensor::sendData() {
 	networkConnection.sendFlexData(sensorId, m_Data);
 }
+
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/ADCResistanceSensor.h

@@ -25,6 +25,8 @@
 #include "sensor.h"
 #include "sensorinterface/SensorInterface.h"
 
+namespace SlimeVR {
+namespace Sensors {
 class ADCResistanceSensor : Sensor {
 public:
 	static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE;
@@ -40,7 +42,7 @@ public:
 			"ADCResistanceSensor",
 			SensorTypeID::ADC_RESISTANCE,
 			id,
-			pin,
+			*(new I2CImpl(0)),
 			0.0f,
 			new SlimeVR::EmptySensorInterface
 		)
@@ -67,3 +69,5 @@ private:
 
 	float m_Data = 0.0f;
 };
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/EmptySensor.h

@@ -31,7 +31,7 @@ namespace Sensors {
 class EmptySensor : public Sensor {
 public:
 	EmptySensor(uint8_t id)
-		: Sensor("EmptySensor", SensorTypeID::Empty, id, 0, 0.0){};
+		: Sensor("EmptySensor", SensorTypeID::Empty, id, *(new I2CImpl(0)), 0.0){};
 	~EmptySensor(){};
 
 	void motionSetup() override final{};

src/sensors/ErroneousSensor.h

@@ -31,7 +31,7 @@ namespace Sensors {
 class ErroneousSensor : public Sensor {
 public:
 	ErroneousSensor(uint8_t id, SensorTypeID type)
-		: Sensor("ErroneousSensor", type, id, 0, 0.0)
+		: Sensor("ErroneousSensor", type, id, *(new I2CImpl(0)), 0.0)
 		, m_ExpectedType(type){};
 	~ErroneousSensor(){};
 

src/sensors/SensorBuilder.h

@@ -0,0 +1,494 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2022 TheDevMinerTV
+
+	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 <map>
+#include <memory>
+#include <optional>
+#include <type_traits>
+
+#include "EmptySensor.h"
+#include "ErroneousSensor.h"
+#include "bmi160sensor.h"
+#include "bno055sensor.h"
+#include "bno080sensor.h"
+#include "globals.h"
+#include "icm20948sensor.h"
+#include "logging/Logger.h"
+#include "mpu6050sensor.h"
+#include "mpu9250sensor.h"
+#include "sensor.h"
+#include "sensorinterface/DirectPinInterface.h"
+#include "sensorinterface/I2CPCAInterface.h"
+#include "sensorinterface/I2CWireSensorInterface.h"
+#include "sensorinterface/MCP23X17PinInterface.h"
+#include "sensorinterface/RegisterInterface.h"
+#include "sensorinterface/SPIImpl.h"
+#include "sensorinterface/SensorInterfaceManager.h"
+#include "sensorinterface/i2cimpl.h"
+#include "softfusion/drivers/bmi270.h"
+#include "softfusion/drivers/icm42688.h"
+#include "softfusion/drivers/icm45605.h"
+#include "softfusion/drivers/icm45686.h"
+#include "softfusion/drivers/lsm6ds3trc.h"
+#include "softfusion/drivers/lsm6dso.h"
+#include "softfusion/drivers/lsm6dsr.h"
+#include "softfusion/drivers/lsm6dsv.h"
+#include "softfusion/drivers/mpu6050.h"
+#include "softfusion/softfusionsensor.h"
+
+#ifndef PRIMARY_IMU_ADDRESS_ONE
+#define PRIMARY_IMU_ADDRESS_ONE 0
+#endif
+
+#ifndef SECONDARY_IMU_ADDRESS_TWO
+#define SECONDARY_IMU_ADDRESS_TWO 0
+#endif
+
+#if USE_RUNTIME_CALIBRATION
+#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
+#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator
+#else
+#include "sensors/softfusion/SoftfusionCalibration.h"
+#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator
+#endif
+
+#if ESP32
+#include "driver/i2c.h"
+#endif
+
+namespace SlimeVR {
+namespace Sensors {
+using SoftFusionLSM6DS3TRC
+	= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SFCALIBRATOR>;
+using SoftFusionICM42688
+	= SoftFusionSensor<SoftFusion::Drivers::ICM42688, SFCALIBRATOR>;
+using SoftFusionBMI270 = SoftFusionSensor<SoftFusion::Drivers::BMI270, SFCALIBRATOR>;
+using SoftFusionLSM6DSV = SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SFCALIBRATOR>;
+using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SFCALIBRATOR>;
+using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SFCALIBRATOR>;
+using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SFCALIBRATOR>;
+using SoftFusionICM45686
+	= SoftFusionSensor<SoftFusion::Drivers::ICM45686, SFCALIBRATOR>;
+using SoftFusionICM45605
+	= SoftFusionSensor<SoftFusion::Drivers::ICM45605, SFCALIBRATOR>;
+class SensorAuto {};
+
+struct SensorBuilder {
+public:
+	SensorManager* m_Manager;
+	SensorBuilder(SensorManager* sensorManager)
+		: m_Manager(sensorManager) {}
+
+	uint8_t buildAllSensors() {
+		SensorInterfaceManager interfaceManager;
+
+		uint8_t sensorID = 0;
+		uint8_t activeSensorCount = 0;
+
+#define NO_PIN nullptr
+#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin)
+#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda)
+#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin)
+#define PCA_WIRE(scl, sda, addr, ch) \
+	interfaceManager.pcaWireInterface().get(scl, sda, addr, ch);
+#define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \
+	*(new SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN))
+
+#define SENSOR_DESC_ENTRY(ImuType, ...)                                              \
+	{                                                                                \
+		do {                                                                         \
+			std::unique_ptr<::Sensor> sensor;                                        \
+			if constexpr (std::is_same<ImuType, SensorAuto>::value) {                \
+				auto sensorType = findSensorType(sensorID, __VA_ARGS__);             \
+				if (sensorType == SensorTypeID::Unknown) {                           \
+					m_Manager->m_Logger.error(                                       \
+						"Can't find sensor type for sensor %d",                      \
+						sensorID                                                     \
+					);                                                               \
+					break;                                                           \
+				} else {                                                             \
+					m_Manager->m_Logger.info(                                        \
+						"Sensor %d automatically detected with %s",                  \
+						sensorID,                                                    \
+						getIMUNameByType(sensorType)                                 \
+					);                                                               \
+					sensor                                                           \
+						= buildSensorDynamically(sensorType, sensorID, __VA_ARGS__); \
+				}                                                                    \
+			} else {                                                                 \
+				sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__);                \
+			}                                                                        \
+			if (sensor->isWorking()) {                                               \
+				m_Manager->m_Logger.info("Sensor %d configured", sensorID);          \
+				activeSensorCount++;                                                 \
+			}                                                                        \
+			m_Manager->m_Sensors.push_back(std::move(sensor));                       \
+		} while (false);                                                             \
+		sensorID++;                                                                  \
+	}
+
+		// Apply descriptor list and expand to entries
+		SENSOR_DESC_LIST;
+
+#define SENSOR_INFO_ENTRY(ImuID, ...) \
+	{ m_Manager->m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); }
+
+		// Apply sensor info list
+		SENSOR_INFO_LIST;
+
+		return activeSensorCount;
+	}
+
+#define BUILD_SENSOR_ARGS \
+	sensorID, imuInterface, rotation, sensorInterface, optional, intPin, extraParam
+
+	std::unique_ptr<::Sensor> buildSensorDynamically(
+		SensorTypeID type,
+		uint8_t sensorID,
+		RegisterInterface& imuInterface,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional,
+		PinInterface* intPin,
+		int extraParam
+	) {
+		switch (type) {
+			// case SensorTypeID::MPU9250:
+			//	return buildSensor<MPU9250Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::BNO080:
+			//	return buildSensor<BNO080Sensor>(BUILD_SENSOR_ARGS);
+			case SensorTypeID::BNO085:
+				return buildSensor<BNO085Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::BNO055:
+			//	return buildSensor<BNO055Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::MPU6050:
+			//	return buildSensor<SoftFusionMPU6050>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			// case SensorTypeID::BNO086:
+			//	return buildSensor<BNO086Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::BMI160:
+			//	return buildSensor<BMI160Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::ICM20948:
+			//	return buildSensor<ICM20948Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::ICM42688:
+			//	return buildSensor<SoftFusionICM42688>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			case SensorTypeID::BMI270:
+				return buildSensor<SoftFusionBMI270>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::LSM6DS3TRC:
+			//	return buildSensor<SoftFusionLSM6DS3TRC>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			case SensorTypeID::LSM6DSV:
+				return buildSensor<SoftFusionLSM6DSV>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::LSM6DSO:
+			//	return buildSensor<SoftFusionLSM6DSO>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			// case SensorTypeID::LSM6DSR:
+			//	return buildSensor<SoftFusionLSM6DSR>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			case SensorTypeID::ICM45686:
+				return buildSensor<SoftFusionICM45686>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::ICM45605:
+			//	return buildSensor<SoftFusionICM45605>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			default:
+				m_Manager->m_Logger.error(
+					"Unable to create sensor with type %s (%d)",
+					getIMUNameByType(type),
+					type
+				);
+		}
+		return std::make_unique<EmptySensor>(sensorID);
+	}
+
+	std::unique_ptr<::Sensor> buildSensorDynamically(
+		SensorTypeID type,
+		uint8_t sensorID,
+		uint8_t imuInterface,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional,
+		PinInterface* intPin,
+		int extraParam
+	) {
+		switch (type) {
+			// case SensorTypeID::MPU9250:
+			//	return buildSensor<MPU9250Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::BNO080:
+			//	return buildSensor<BNO080Sensor>(BUILD_SENSOR_ARGS);
+			case SensorTypeID::BNO085:
+				return buildSensor<BNO085Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::BNO055:
+			//	return buildSensor<BNO055Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::MPU6050:
+			//	return buildSensor<SoftFusionMPU6050>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			// case SensorTypeID::BNO086:
+			//	return buildSensor<BNO086Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::BMI160:
+			//	return buildSensor<BMI160Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::ICM20948:
+			//	return buildSensor<ICM20948Sensor>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::ICM42688:
+			//	return buildSensor<SoftFusionICM42688>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			case SensorTypeID::BMI270:
+				return buildSensor<SoftFusionBMI270>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::LSM6DS3TRC:
+			//	return buildSensor<SoftFusionLSM6DS3TRC>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			case SensorTypeID::LSM6DSV:
+				return buildSensor<SoftFusionLSM6DSV>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::LSM6DSO:
+			//	return buildSensor<SoftFusionLSM6DSO>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			// case SensorTypeID::LSM6DSR:
+			//	return buildSensor<SoftFusionLSM6DSR>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			case SensorTypeID::ICM45686:
+				return buildSensor<SoftFusionICM45686>(BUILD_SENSOR_ARGS);
+			// case SensorTypeID::ICM45605:
+			//	return buildSensor<SoftFusionICM45605>(
+			//		BUILD_SENSOR_ARGS
+			//	);
+			default:
+				m_Manager->m_Logger.error(
+					"Unable to create sensor with type %s (%d)",
+					getIMUNameByType(type),
+					type
+				);
+		}
+		return std::make_unique<EmptySensor>(sensorID);
+	}
+
+	SensorTypeID findSensorType(
+		uint8_t sensorID,
+		uint8_t imuAddress,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional,
+		PinInterface* intPin,
+		int extraParam
+	) {
+		sensorInterface->init();
+		sensorInterface->swapIn();
+		// if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuAddress))
+		// { 	return SensorTypeID::LSM6DS3TRC;
+		// }
+		// if (SoftFusionICM42688::checkPresent(sensorID, imuAddress)) {
+		//	return SensorTypeID::ICM42688;
+		//}
+		if (SoftFusionBMI270::checkPresent(sensorID, imuAddress)) {
+			return SensorTypeID::BMI270;
+		}
+		if (SoftFusionLSM6DSV::checkPresent(sensorID, imuAddress)) {
+			return SensorTypeID::LSM6DSV;
+		}
+		// if (SoftFusionLSM6DSO::checkPresent(sensorID, imuAddress)) {
+		//	return SensorTypeID::LSM6DSO;
+		// }
+		// if (SoftFusionLSM6DSR::checkPresent(sensorID, imuAddress)) {
+		//	return SensorTypeID::LSM6DSR;
+		// }
+		// if (SoftFusionMPU6050::checkPresent(sensorID, imuAddress)) {
+		//	return SensorTypeID::MPU6050;
+		// }
+		if (SoftFusionICM45686::checkPresent(sensorID, imuAddress)) {
+			return SensorTypeID::ICM45686;
+		}
+		return BNO080Sensor::checkIfPresent(sensorID, imuAddress, intPin);
+		// if (SoftFusionICM45605::checkPresent(sensorID, imuAddress)) {
+		//	return SensorTypeID::ICM45605;
+		// }
+
+		return SensorTypeID::Unknown;
+	}
+
+	SensorTypeID findSensorType(
+		uint8_t sensorID,
+		RegisterInterface& imuInterface,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional,
+		PinInterface* intPin,
+		int extraParam
+	) {
+		sensorInterface->init();
+		sensorInterface->swapIn();
+		// if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuInterface))
+		// { 	return SensorTypeID::LSM6DS3TRC;
+		// }
+		// if (SoftFusionICM42688::checkPresent(sensorID, imuInterface)) {
+		//	return SensorTypeID::ICM42688;
+		//}
+		if (SoftFusionBMI270::checkPresent(sensorID, imuInterface)) {
+			return SensorTypeID::BMI270;
+		}
+		if (SoftFusionLSM6DSV::checkPresent(sensorID, imuInterface)) {
+			return SensorTypeID::LSM6DSV;
+		}
+		// if (SoftFusionLSM6DSO::checkPresent(sensorID, imuInterface)) {
+		//	return SensorTypeID::LSM6DSO;
+		// }
+		// if (SoftFusionLSM6DSR::checkPresent(sensorID, imuInterface)) {
+		//	return SensorTypeID::LSM6DSR;
+		// }
+		// if (SoftFusionMPU6050::checkPresent(sensorID, imuInterface)) {
+		//	return SensorTypeID::MPU6050;
+		// }
+		if (SoftFusionICM45686::checkPresent(sensorID, imuInterface)) {
+			return SensorTypeID::ICM45686;
+		}
+		return BNO080Sensor::checkIfPresent(sensorID, sensorInterface, intPin);
+		// if (SoftFusionICM45605::checkPresent(sensorID, imuInterface)) {
+		//	return SensorTypeID::ICM45605;
+		// }
+
+		return SensorTypeID::Unknown;
+	}
+
+	template <typename ImuType>
+	std::unique_ptr<::Sensor> buildSensor(
+		uint8_t sensorID,
+		RegisterInterface& imuInterface,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional = false,
+		PinInterface* intPin = nullptr,
+		int extraParam = 0
+	) {
+		return buildSensorReal<ImuType>(
+			sensorID,
+			imuInterface,
+			rotation,
+			sensorInterface,
+			optional,
+			intPin,
+			extraParam
+		);
+	}
+
+	template <typename ImuType>
+	std::unique_ptr<::Sensor> buildSensor(
+		uint8_t sensorID,
+		uint8_t imuAddress,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional = false,
+		PinInterface* intPin = nullptr,
+		int extraParam = 0
+	) {
+		uint8_t address = imuAddress > 0 ? imuAddress : ImuType::Address + sensorID;
+		return buildSensorReal<ImuType>(
+			sensorID,
+			*(new I2CImpl(address)),
+			rotation,
+			sensorInterface,
+			optional,
+			intPin,
+			extraParam
+		);
+	}
+
+	template <typename ImuType>
+	std::unique_ptr<::Sensor> buildSensorReal(
+		uint8_t sensorID,
+		RegisterInterface& imuInterface,
+		float rotation,
+		SensorInterface* sensorInterface,
+		bool optional = false,
+		PinInterface* intPin = nullptr,
+		int extraParam = 0
+	) {
+		m_Manager->m_Logger.trace(
+			"Building IMU with: id=%d,\n\
+						address=%s, rotation=%f,\n\
+						interface=%s, int=%s, extraParam=%d, optional=%d",
+			sensorID,
+			imuInterface.toString(),
+			rotation,
+			sensorInterface,
+			intPin,
+			extraParam,
+			optional
+		);
+
+		// Now start detecting and building the IMU
+		std::unique_ptr<::Sensor> sensor;
+
+		// Init I2C bus for each sensor upon startup
+		sensorInterface->init();
+		sensorInterface->swapIn();
+
+		if (imuInterface.hasSensorOnBus()) {
+			m_Manager->m_Logger.trace(
+				"Sensor %d found at address %s",
+				sensorID + 1,
+				imuInterface.toString()
+			);
+		} else {
+			if (!optional) {
+				m_Manager->m_Logger.error(
+					"Mandatory sensor %d not found at address %s",
+					sensorID + 1,
+					imuInterface.toString()
+				);
+				sensor = std::make_unique<ErroneousSensor>(sensorID, ImuType::TypeID);
+			} else {
+				m_Manager->m_Logger.debug(
+					"Optional sensor %d not found at address %s",
+					sensorID + 1,
+					imuInterface.toString()
+				);
+				sensor = std::make_unique<EmptySensor>(sensorID);
+			}
+			return sensor;
+		}
+
+		sensor = std::make_unique<ImuType>(
+			sensorID,
+			imuInterface,
+			rotation,
+			sensorInterface,
+			intPin,
+			extraParam
+		);
+
+		sensor->motionSetup();
+		return sensor;
+	}
+};
+}  // namespace Sensors
+}  // namespace SlimeVR

src/sensors/SensorManager.cpp

@@ -23,107 +23,19 @@
 
 #include "SensorManager.h"
 
-#include <map>
-#include <type_traits>
-
-#include "bmi160sensor.h"
-#include "bno055sensor.h"
-#include "bno080sensor.h"
-#include "icm20948sensor.h"
-#include "mpu6050sensor.h"
-#include "mpu9250sensor.h"
-#include "sensorinterface/SensorInterfaceManager.h"
-#include "sensors/softfusion/SoftfusionCalibration.h"
-#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h"
-#include "softfusion/drivers/bmi270.h"
-#include "softfusion/drivers/icm42688.h"
-#include "softfusion/drivers/icm45605.h"
-#include "softfusion/drivers/icm45686.h"
-#include "softfusion/drivers/lsm6ds3trc.h"
-#include "softfusion/drivers/lsm6dso.h"
-#include "softfusion/drivers/lsm6dsr.h"
-#include "softfusion/drivers/lsm6dsv.h"
-#include "softfusion/drivers/mpu6050.h"
-#include "softfusion/i2cimpl.h"
-#include "softfusion/softfusionsensor.h"
-
-#if ESP32
-#include "driver/i2c.h"
-#endif
-
-#if USE_RUNTIME_CALIBRATION
-#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator
-#else
-#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator
-#endif
+#include "SensorBuilder.h"
 
 namespace SlimeVR {
 namespace Sensors {
-using SoftFusionLSM6DS3TRC = SoftFusionSensor<
-	SoftFusion::Drivers::LSM6DS3TRC,
-	SoftFusion::I2CImpl,
-	SFCALIBRATOR>;
-using SoftFusionICM42688 = SoftFusionSensor<
-	SoftFusion::Drivers::ICM42688,
-	SoftFusion::I2CImpl,
-	SFCALIBRATOR>;
-using SoftFusionBMI270
-	= SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl, SFCALIBRATOR>;
-using SoftFusionLSM6DSV
-	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl, SFCALIBRATOR>;
-using SoftFusionLSM6DSO
-	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl, SFCALIBRATOR>;
-using SoftFusionLSM6DSR
-	= SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl, SFCALIBRATOR>;
-using SoftFusionMPU6050
-	= SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl, SFCALIBRATOR>;
-using SoftFusionICM45686 = SoftFusionSensor<
-	SoftFusion::Drivers::ICM45686,
-	SoftFusion::I2CImpl,
-	SFCALIBRATOR>;
-using SoftFusionICM45605 = SoftFusionSensor<
-	SoftFusion::Drivers::ICM45605,
-	SoftFusion::I2CImpl,
-	SFCALIBRATOR>;
 
 void SensorManager::setup() {
-	SensorInterfaceManager interfaceManager;
-
-	uint8_t sensorID = 0;
-	uint8_t activeSensorCount = 0;
 	if (m_MCP.begin_I2C()) {
 		m_Logger.info("MCP initialized");
 	}
 
-#define NO_PIN nullptr
-#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin)
-#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda)
-#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin)
-#define PCA_WIRE(scl, sda, addr, ch) \
-	interfaceManager.pcaWireInterface().get(scl, sda, addr, ch);
+	SensorBuilder sensorBuilder = SensorBuilder(this);
+	uint8_t activeSensorCount = sensorBuilder.buildAllSensors();
 
-#define SENSOR_DESC_ENTRY(ImuType, ...)                            \
-	{                                                              \
-		auto sensor = buildSensor<ImuType>(sensorID, __VA_ARGS__); \
-		if (sensor->isWorking()) {                                 \
-			m_Logger.info("Sensor %d configured", sensorID + 1);   \
-			activeSensorCount++;                                   \
-		}                                                          \
-		m_Sensors.push_back(std::move(sensor));                    \
-		sensorID++;                                                \
-	}
-
-	// Apply descriptor list and expand to entries
-	SENSOR_DESC_LIST;
-
-#define SENSOR_INFO_ENTRY(ImuID, ...) \
-	{ m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); }
-	SENSOR_INFO_LIST;
-
-#undef SENSOR_DESC_ENTRY
-#undef NO_PIN
-#undef DIRECT_PIN
-#undef DIRECT_WIRE
 	m_Logger.info("%d sensor(s) configured", activeSensorCount);
 	// Check and scan i2c if no sensors active
 	if (activeSensorCount == 0) {
@@ -209,5 +121,6 @@ void SensorManager::update() {
 	networkConnection.endBundle();
 #endif
 }
+
 }  // namespace Sensors
 }  // namespace SlimeVR

src/sensors/SensorManager.h

@@ -31,32 +31,16 @@
 #include "ErroneousSensor.h"
 #include "globals.h"
 #include "logging/Logger.h"
-#include "sensor.h"
-#include "sensoraddress.h"
 #include "sensorinterface/DirectPinInterface.h"
 #include "sensorinterface/I2CPCAInterface.h"
 #include "sensorinterface/I2CWireSensorInterface.h"
 #include "sensorinterface/MCP23X17PinInterface.h"
+#include "sensorinterface/RegisterInterface.h"
+#include "sensorinterface/i2cimpl.h"
 
 namespace SlimeVR {
 namespace Sensors {
 
-#ifndef PRIMARY_IMU_ADDRESS_ONE
-#define PRIMARY_IMU_ADDRESS_ONE true
-#endif
-
-#ifndef PRIMARY_IMU_ADDRESS_TWO
-#define PRIMARY_IMU_ADDRESS_TWO false
-#endif
-
-#ifndef SECONDARY_IMU_ADDRESS_TWO
-#define SECONDARY_IMU_ADDRESS_TWO false
-#endif
-
-#ifndef SECONDARY_IMU_ADDRESS_ONE
-#define SECONDARY_IMU_ADDRESS_ONE true
-#endif
-
 class SensorManager {
 public:
 	SensorManager()
@@ -80,81 +64,9 @@ private:
 	std::vector<std::unique_ptr<::Sensor>> m_Sensors;
 	Adafruit_MCP23X17 m_MCP;
 
-	template <typename ImuType>
-	std::unique_ptr<::Sensor> buildSensor(
-		uint8_t sensorID,
-		ImuAddress imuAddress,
-		float rotation,
-		SensorInterface* sensorInterface,
-		bool optional = false,
-		PinInterface* intPin = nullptr,
-		int extraParam = 0
-	) {
-		uint8_t i2cAddress = imuAddress.getAddress(ImuType::Address);
-		m_Logger.trace(
-			"Building IMU with: id=%d,\n\
-						address=0x%02X, rotation=%f,\n\
-						interface=%s, int=%s, extraParam=%d, optional=%d",
-			sensorID,
-			i2cAddress,
-			rotation,
-			sensorInterface,
-			intPin,
-			extraParam,
-			optional
-		);
-
-		// Now start detecting and building the IMU
-		std::unique_ptr<::Sensor> sensor;
-
-		// Init I2C bus for each sensor upon startup
-		sensorInterface->init();
-		sensorInterface->swapIn();
-
-		if (I2CSCAN::hasDevOnBus(i2cAddress)) {
-			m_Logger
-				.trace("Sensor %d found at address 0x%02X", sensorID + 1, i2cAddress);
-		} else if (I2CSCAN::hasDevOnBus(i2cAddress
-				   )) {  // scan again because ICM45* may not respond on first I2C
-						 // transaction
-			m_Logger.trace(
-				"Sensor %d found at address 0x%02X on second scan",
-				sensorID + 1,
-				i2cAddress
-			);
-		} else {
-			if (!optional) {
-				m_Logger.error(
-					"Mandatory sensor %d not found at address 0x%02X",
-					sensorID + 1,
-					i2cAddress
-				);
-				sensor = std::make_unique<ErroneousSensor>(sensorID, ImuType::TypeID);
-			} else {
-				m_Logger.debug(
-					"Optional sensor %d not found at address 0x%02X",
-					sensorID + 1,
-					i2cAddress
-				);
-				sensor = std::make_unique<EmptySensor>(sensorID);
-			}
-			return sensor;
-		}
-
-		sensor = std::make_unique<ImuType>(
-			sensorID,
-			i2cAddress,
-			rotation,
-			sensorInterface,
-			intPin,
-			extraParam
-		);
-
-		sensor->motionSetup();
-		return sensor;
-	}
-
 	uint32_t m_LastBundleSentAtMicros = micros();
+
+	friend class SensorBuilder;
 };
 }  // namespace Sensors
 }  // namespace SlimeVR

src/sensors/bmi160sensor.h

@@ -139,7 +139,7 @@ public:
 
 	BMI160Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface*,
@@ -149,7 +149,7 @@ public:
 			"BMI160Sensor",
 			SensorTypeID::BMI160,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface
 		)

src/sensors/bno055sensor.h

@@ -35,7 +35,7 @@ public:
 
 	BNO055Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface*,
@@ -45,7 +45,7 @@ public:
 			"BNO055Sensor",
 			SensorTypeID::BNO055,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface
 		){};

src/sensors/bno080sensor.h

@@ -25,6 +25,7 @@
 #define SENSORS_BNO080SENSOR_H
 
 #include <BNO080.h>
+#include <i2cscan.h>
 
 #include "sensor.h"
 
@@ -37,7 +38,7 @@ public:
 
 	BNO080Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface* intPin,
@@ -47,7 +48,7 @@ public:
 			"BNO080Sensor",
 			SensorTypeID::BNO080,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface
 		)
@@ -63,19 +64,43 @@ public:
 	bool isFlagSupported(SensorToggles toggle) const final;
 	void sendTempIfNeeded();
 
+	static SensorTypeID checkIfPresent(
+		uint8_t sensorID,
+		SlimeVR::SensorInterface* sensorInterface,
+		PinInterface* intPin
+	) {
+		// Lazy check for if BNO is present, we only check if I2C has an address here
+		if (I2CSCAN::hasDevOnBus(Address + sensorID)) {
+			return SensorTypeID::BNO085;  // Assume it's 085, more precise diff will
+										  // require talking to it
+		}
+		return SensorTypeID::Unknown;
+	}
+
+	static SensorTypeID
+	checkIfPresent(uint8_t sensorID, uint8_t imuAddress, PinInterface* intPin) {
+		uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID;
+		// Lazy check for if BNO is present, we only check if I2C has an address here
+		if (I2CSCAN::hasDevOnBus(address)) {
+			return SensorTypeID::BNO085;  // Assume it's 085, more precise diff will
+										  // require talking to it
+		}
+		return SensorTypeID::Unknown;
+	}
+
 protected:
 	// forwarding constructor
 	BNO080Sensor(
 		const char* sensorName,
 		SensorTypeID imuId,
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface* intPin,
 		int
 	)
-		: Sensor(sensorName, imuId, id, i2cAddress, rotation, sensorInterface)
+		: Sensor(sensorName, imuId, id, registerInterface, rotation, sensorInterface)
 		, m_IntPin(intPin){};
 
 private:
@@ -107,7 +132,7 @@ public:
 	static constexpr auto TypeID = SensorTypeID::BNO085;
 	BNO085Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface* intPin,
@@ -117,7 +142,7 @@ public:
 			"BNO085Sensor",
 			SensorTypeID::BNO085,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface,
 			intPin,
@@ -130,7 +155,7 @@ public:
 	static constexpr auto TypeID = SensorTypeID::BNO086;
 	BNO086Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface* intPin,
@@ -140,7 +165,7 @@ public:
 			"BNO086Sensor",
 			SensorTypeID::BNO086,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface,
 			intPin,

src/sensors/icm20948sensor.h

@@ -35,7 +35,7 @@ public:
 
 	ICM20948Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface*,
@@ -45,7 +45,7 @@ public:
 			"ICM20948Sensor",
 			SensorTypeID::ICM20948,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface
 		) {}

src/sensors/mpu6050sensor.h

@@ -36,7 +36,7 @@ public:
 
 	MPU6050Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface*,
@@ -46,7 +46,7 @@ public:
 			"MPU6050Sensor",
 			SensorTypeID::MPU6050,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface
 		){};

src/sensors/mpu9250sensor.h

@@ -49,7 +49,7 @@ public:
 
 	MPU9250Sensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface,
 		PinInterface*,
@@ -59,7 +59,7 @@ public:
 			"MPU9250Sensor",
 			SensorTypeID::MPU9250,
 			id,
-			i2cAddress,
+			registerInterface,
 			rotation,
 			sensorInterface
 		)

src/sensors/sensor.cpp

@@ -33,6 +33,7 @@ SensorStatus Sensor::getSensorState() {
 
 void Sensor::setAcceleration(Vector3 a) {
 	acceleration = a;
+	sensorOffset.sandwitch(acceleration);
 	newAcceleration = true;
 }
 

src/sensors/sensor.h

@@ -35,7 +35,9 @@
 #include "configuration/Configuration.h"
 #include "globals.h"
 #include "logging/Logger.h"
+#include "sensorinterface/RegisterInterface.h"
 #include "sensorinterface/SensorInterface.h"
+#include "sensorinterface/i2cimpl.h"
 #include "status/TPSCounter.h"
 #include "utils.h"
 
@@ -54,12 +56,12 @@ public:
 		const char* sensorName,
 		SensorTypeID type,
 		uint8_t id,
-		uint8_t address,
+		SlimeVR::Sensors::RegisterInterface& registerInterface,
 		float rotation,
 		SlimeVR::SensorInterface* sensorInterface = nullptr
 	)
 		: m_hwInterface(sensorInterface)
-		, addr(address)
+		, m_RegisterInterface(registerInterface)
 		, sensorId(id)
 		, sensorType(type)
 		, sensorOffset({Quat(Vector3(0, 0, 1), rotation)})
@@ -67,6 +69,7 @@ public:
 		char buf[4];
 		sprintf(buf, "%u", id);
 		m_Logger.setTag(buf);
+		addr = registerInterface.getAddress();
 	}
 
 	virtual ~Sensor(){};
@@ -107,12 +110,18 @@ public:
 		m_SensorPosition = sensorPosition;
 	};
 
+	virtual void deinitialize() {
+		// Sensors must implement deinitialize if ON_OFF_BUTTON so that the
+		// sensor can go to sleep
+	}
+
 	TPSCounter m_tpsCounter;
 	TPSCounter m_dataCounter;
 	SlimeVR::SensorInterface* m_hwInterface = nullptr;
 
 protected:
-	uint8_t addr = 0;
+	SlimeVR::Sensors::RegisterInterface& m_RegisterInterface;
+	uint8_t addr;
 	uint8_t sensorId = 0;
 	SensorTypeID sensorType = SensorTypeID::Unknown;
 	bool working = false;

src/sensors/softfusion/drivers/bmi270.h

@@ -29,6 +29,7 @@
 #include <cstring>
 #include <limits>
 
+#include "../../../sensorinterface/RegisterInterface.h"
 #include "bmi270fw.h"
 #include "vqf.h"
 
@@ -39,7 +40,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // Gyroscope ODR = 400Hz, accel ODR = 100Hz
 // Timestamps reading are not used
 
-template <typename I2CImpl>
 struct BMI270 {
 	static constexpr uint8_t Address = 0x68;
 	static constexpr auto Name = "BMI270";
@@ -68,13 +68,13 @@ struct BMI270 {
 		uint8_t x, y, z;
 	};
 
-	I2CImpl i2c;
-	SlimeVR::Logging::Logger& logger;
-	int8_t zxFactor;
-	BMI270(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: i2c(i2c)
-		, logger(logger)
-		, zxFactor(0) {}
+	RegisterInterface& m_RegisterInterface;
+	SlimeVR::Logging::Logger& m_Logger;
+	int8_t m_zxFactor;
+	BMI270(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: m_RegisterInterface(registerInterface)
+		, m_Logger(logger)
+		, m_zxFactor(0) {}
 
 	struct Regs {
 		struct WhoAmI {
@@ -244,14 +244,20 @@ struct BMI270 {
 
 	bool restartAndInit() {
 		// perform initialization step
-		i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset);
+		m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset);
 		delay(12);
 		// disable power saving
-		i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving);
+		m_RegisterInterface.writeReg(
+			Regs::PwrConf::reg,
+			Regs::PwrConf::valueNoPowerSaving
+		);
 		delay(1);
 
 		// firmware upload
-		i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit);
+		m_RegisterInterface.writeReg(
+			Regs::InitCtrl::reg,
+			Regs::InitCtrl::valueStartInit
+		);
 		for (uint16_t pos = 0; pos < sizeof(bmi270_firmware);) {
 			// tell the device current position
 
@@ -261,57 +267,63 @@ struct BMI270 {
 
 			const uint16_t pos_words = pos >> 1;  // convert current position to words
 			const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00);
-			i2c.writeReg16(Regs::InitAddr, position);
+			m_RegisterInterface.writeReg16(Regs::InitAddr, position);
 			// write actual payload chunk
 			const uint16_t burstWrite = std::min(
-				sizeof(bmi270_firmware) - pos,
-				I2CImpl::MaxTransactionLength
+				static_cast<size_t>(sizeof(bmi270_firmware) - pos),
+				RegisterInterface::MaxTransactionLength
 			);
-			i2c.writeBytes(
+			m_RegisterInterface.writeBytes(
 				Regs::InitData,
 				burstWrite,
 				const_cast<uint8_t*>(bmi270_firmware + pos)
 			);
 			pos += burstWrite;
 		}
-		i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit);
+		m_RegisterInterface.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit);
 		delay(140);
 
 		// leave fifo_self_wakeup enabled
-		i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup);
+		m_RegisterInterface.writeReg(
+			Regs::PwrConf::reg,
+			Regs::PwrConf::valueFifoSelfWakeup
+		);
 		// check if IMU initialized correctly
-		if (!(i2c.readReg(Regs::InternalStatus::reg)
+		if (!(m_RegisterInterface.readReg(Regs::InternalStatus::reg)
 			  & Regs::InternalStatus::initializedBit)) {
 			// firmware upload fail or sensor not initialized
 			return false;
 		}
 
 		// read zx factor used to reduce gyro cross-sensitivity error
-		const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas);
+		const uint8_t zx_factor_reg = m_RegisterInterface.readReg(Regs::RaGyrCas);
 		const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80;
-		zxFactor = static_cast<int8_t>(zx_factor_reg | sign_byte);
+		m_zxFactor = static_cast<int8_t>(zx_factor_reg | sign_byte);
 		return true;
 	}
 
 	void setNormalConfig(MotionlessCalibrationData& gyroSensitivity) {
-		i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value);
-		i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value);
+		m_RegisterInterface.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value);
+		m_RegisterInterface.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value);
 
-		i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value);
-		i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value);
+		m_RegisterInterface.writeReg(Regs::AccConf::reg, Regs::AccConf::value);
+		m_RegisterInterface.writeReg(Regs::AccRange::reg, Regs::AccRange::value);
 
 		if (gyroSensitivity.valid) {
-			i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value);
-			i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x);
+			m_RegisterInterface.writeReg(Regs::Offset6::reg, Regs::Offset6::value);
+			m_RegisterInterface.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x);
 		}
 
-		i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
+		m_RegisterInterface.writeReg(
+			Regs::PwrCtrl::reg,
+			Regs::PwrCtrl::valueGyrAccTempOn
+		);
 		delay(100);  // power up delay
-		i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
-		i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
+		m_RegisterInterface.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
+		m_RegisterInterface.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
 
 		delay(4);
-		i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush);
+		m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush);
 		delay(2);
 	}
 
@@ -325,48 +337,63 @@ struct BMI270 {
 		return true;
 	}
 
+	void deinitialize() {
+		m_RegisterInterface.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueOff);
+	}
+
 	bool motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) {
 		// perfrom gyroscope motionless sensitivity calibration (CRT)
 		// need to start from clean state according to spec
 		restartAndInit();
 		// only Accel ON
-		i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn);
+		m_RegisterInterface.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn);
 		delay(100);
-		i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning);
-		i2c.writeReg(Regs::FeatPage, 1);
-		i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT);
-		i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger);
+		m_RegisterInterface.writeReg(
+			Regs::GyrCrtConf::reg,
+			Regs::GyrCrtConf::valueRunning
+		);
+		m_RegisterInterface.writeReg(Regs::FeatPage, 1);
+		m_RegisterInterface.writeReg16(
+			Regs::GTrig1::reg,
+			Regs::GTrig1::valueTriggerCRT
+		);
+		m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger);
 		delay(200);
 
-		while (i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) {
-			logger.info("CRT running. Do not move tracker!");
+		while (m_RegisterInterface.readReg(Regs::GyrCrtConf::reg)
+			   == Regs::GyrCrtConf::valueRunning) {
+			m_Logger.info("CRT running. Do not move tracker!");
 			delay(200);
 		}
 
-		i2c.writeReg(Regs::FeatPage, 0);
+		m_RegisterInterface.writeReg(Regs::FeatPage, 0);
 
-		uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg)
+		uint8_t status = m_RegisterInterface.readReg(Regs::GyrGainStatus::reg)
 					  >> Regs::GyrGainStatus::statusOffset;
 		// turn gyroscope back on
-		i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
+		m_RegisterInterface.writeReg(
+			Regs::PwrCtrl::reg,
+			Regs::PwrCtrl::valueGyrAccTempOn
+		);
 		delay(100);
 
 		bool success;
 
 		if (status != 0) {
-			logger.error(
+			m_Logger.error(
 				"CRT failed with status 0x%x. Recalibrate again to enable CRT.",
 				status
 			);
 			if (status == 0x03) {
-				logger.error("Reason: tracker was moved during CRT!");
+				m_Logger.error("Reason: tracker was moved during CRT!");
 			}
 
 			success = false;
 		} else {
 			std::array<uint8_t, 3> crt_values;
-			i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data());
-			logger.debug(
+			m_RegisterInterface
+				.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data());
+			m_Logger.debug(
 				"CRT finished successfully, result 0x%x, 0x%x, 0x%x",
 				crt_values[0],
 				crt_values[1],
@@ -395,11 +422,12 @@ struct BMI270 {
 		// temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9
 		// degrees C (0x7FFF)
 		constexpr float TempStep = 128. / 65535;
-		const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
+		const auto value
+			= static_cast<int16_t>(m_RegisterInterface.readReg16(Regs::TempData));
 		return static_cast<float>(value) * TempStep + 23.0f;
 	}
 
-	using FifoBuffer = std::array<uint8_t, I2CImpl::MaxTransactionLength>;
+	using FifoBuffer = std::array<uint8_t, RegisterInterface::MaxTransactionLength>;
 	FifoBuffer read_buffer;
 
 	template <typename T>
@@ -416,13 +444,14 @@ struct BMI270 {
 		GyroCall&& processGyroSample,
 		TempCall&& processTempSample
 	) {
-		const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
+		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
 		const auto bytes_to_read = std::min(
 			static_cast<size_t>(read_buffer.size()),
 			static_cast<size_t>(fifo_bytes)
 		);
-		i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+		m_RegisterInterface
+			.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
 
 		for (uint32_t i = 0u; i < bytes_to_read;) {
 			const uint8_t header = getFromFifo<uint8_t>(i, read_buffer);
@@ -451,7 +480,7 @@ struct BMI270 {
 					gyro[0] = std::clamp(
 						static_cast<int32_t>(gyro[0])
 							- static_cast<int16_t>(
-								(static_cast<int32_t>(zxFactor) * gyro[2]) / 512
+								(static_cast<int32_t>(m_zxFactor) * gyro[2]) / 512
 							),
 						static_cast<int32_t>(ShortLimit::min()),
 						static_cast<int32_t>(ShortLimit::max())

src/sensors/softfusion/drivers/icm42688.h

@@ -36,7 +36,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // Gyroscope ODR = 500Hz, accel ODR = 100Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
-template <typename I2CImpl>
 struct ICM42688 {
 	static constexpr uint8_t Address = 0x68;
 	static constexpr auto Name = "ICM-42688";
@@ -66,11 +65,11 @@ struct ICM42688 {
 		.restThAcc = 0.196f,
 	};
 
-	I2CImpl i2c;
-	SlimeVR::Logging::Logger& logger;
-	ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: i2c(i2c)
-		, logger(logger) {}
+	RegisterInterface& m_RegisterInterface;
+	SlimeVR::Logging::Logger& m_Logger;
+	ICM42688(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: m_RegisterInterface(registerInterface)
+		, m_Logger(logger) {}
 
 	struct Regs {
 		struct WhoAmI {
@@ -143,15 +142,18 @@ struct ICM42688 {
 
 	bool initialize() {
 		// perform initialization step
-		i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset);
+		m_RegisterInterface.writeReg(
+			Regs::DeviceConfig::reg,
+			Regs::DeviceConfig::valueSwReset
+		);
 		delay(20);
 
-		i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value);
-		i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
-		i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
-		i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
-		i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
-		i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value);
+		m_RegisterInterface.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value);
+		m_RegisterInterface.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
+		m_RegisterInterface.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
+		m_RegisterInterface.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
+		m_RegisterInterface.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value);
+		m_RegisterInterface.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value);
 		delay(1);
 
 		return true;
@@ -163,7 +165,7 @@ struct ICM42688 {
 		GyroCall&& processGyroSample,
 		TempCall&& processTemperatureSample
 	) {
-		const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
+		const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
 
 		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
 		const auto bytes_to_read = std::min(
@@ -171,7 +173,8 @@ struct ICM42688 {
 									   static_cast<size_t>(fifo_bytes)
 								   )
 								 / FullFifoEntrySize * FullFifoEntrySize;
-		i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+		m_RegisterInterface
+			.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
 		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
 			FifoEntryAligned entry;
 			memcpy(

src/sensors/softfusion/drivers/icm45605.h

@@ -35,8 +35,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
-template <typename I2CImpl>
-struct ICM45605 : public ICM45Base<I2CImpl> {
+struct ICM45605 : public ICM45Base {
 	static constexpr auto Name = "ICM-45605";
 	static constexpr auto Type = SensorTypeID::ICM45605;
 
@@ -48,8 +47,8 @@ struct ICM45605 : public ICM45Base<I2CImpl> {
 		.restThAcc = 0.0098f,
 	};
 
-	ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: ICM45Base<I2CImpl>{i2c, logger} {}
+	ICM45605(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: ICM45Base{registerInterface, logger} {}
 
 	struct Regs {
 		struct WhoAmI {
@@ -59,8 +58,8 @@ struct ICM45605 : public ICM45Base<I2CImpl> {
 	};
 
 	bool initialize() {
-		ICM45Base<I2CImpl>::softResetIMU();
-		return ICM45Base<I2CImpl>::initializeBase();
+		ICM45Base::softResetIMU();
+		return ICM45Base::initializeBase();
 	}
 };
 

src/sensors/softfusion/drivers/icm45686.h

@@ -35,21 +35,26 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
-template <typename I2CImpl>
-struct ICM45686 : public ICM45Base<I2CImpl> {
+struct ICM45686 : public ICM45Base {
 	static constexpr auto Name = "ICM-45686";
 	static constexpr auto Type = SensorTypeID::ICM45686;
 
 	static constexpr VQFParams SensorVQFParams{
-		.motionBiasEstEnabled = true,
-		.biasSigmaInit = 0.5f,
-		.biasClip = 1.0f,
-		.restThGyr = 0.5f,
-		.restThAcc = 0.196f,
+		.tauAcc = 7.171490,
+		.biasSigmaInit = 0.337976,
+		.biasForgettingTime = 352.235500,
+		.biasClip = 5.0,
+		.biasSigmaMotion = 0.985346,
+		.biasVerticalForgettingFactor = 0.007959,
+		.biasSigmaRest = 0.028897,
+		.restMinT = 4.648680,
+		.restFilterTau = 1.900166,
+		.restThGyr = 2.620598,
+		.restThAcc = 2.142593,
 	};
 
-	ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: ICM45Base<I2CImpl>{i2c, logger} {}
+	ICM45686(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: ICM45Base{registerInterface, logger} {}
 
 	struct Regs {
 		struct WhoAmI {
@@ -68,13 +73,11 @@ struct ICM45686 : public ICM45Base<I2CImpl> {
 		};
 	};
 
-	using ICM45Base<I2CImpl>::i2c;
-
 	bool initialize() {
-		ICM45Base<I2CImpl>::softResetIMU();
-		i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value);
-		i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value);
-		return ICM45Base<I2CImpl>::initializeBase();
+		ICM45Base::softResetIMU();
+		m_RegisterInterface.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value);
+		m_RegisterInterface.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value);
+		return ICM45Base::initializeBase();
 	}
 };
 

src/sensors/softfusion/drivers/icm45base.h

@@ -24,6 +24,8 @@
 #include <array>
 #include <cstdint>
 
+#include "../../../sensorinterface/RegisterInterface.h"
+
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
 // Driver uses acceleration range at 32g
@@ -33,7 +35,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz
 // Timestamps reading not used, as they're useless (constant predefined increment)
 
-template <typename I2CImpl>
 struct ICM45Base {
 	static constexpr uint8_t Address = 0x68;
 
@@ -53,11 +54,11 @@ struct ICM45Base {
 
 	static constexpr bool Uses32BitSensorData = true;
 
-	I2CImpl i2c;
-	SlimeVR::Logging::Logger& logger;
-	ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: i2c(i2c)
-		, logger(logger) {}
+	RegisterInterface& m_RegisterInterface;
+	SlimeVR::Logging::Logger& m_Logger;
+	ICM45Base(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: m_RegisterInterface(registerInterface)
+		, m_Logger(logger) {}
 
 	struct BaseRegs {
 		static constexpr uint8_t TempData = 0x0c;
@@ -82,9 +83,10 @@ struct ICM45Base {
 		struct FifoConfig0 {
 			static constexpr uint8_t reg = 0x1d;
 			static constexpr uint8_t value
-				= (0b01 << 6) | (0b011111);  // stream to FIFO mode, FIFO depth
-											 // 8k bytes <-- this disables all APEX
-											 // features, but we don't need them
+				= (0b10 << 6)
+				| (0b011111);  // Stop-on-full FIFO mode -- See AN-000364 section 2.16
+							   // for why this is required FIFO depth = 8k bytes, this
+							   // disables all APEX features, but we don't need them
 		};
 
 		struct FifoConfig3 {
@@ -100,6 +102,7 @@ struct ICM45Base {
 			static constexpr uint8_t reg = 0x10;
 			static constexpr uint8_t value
 				= 0b11 | (0b11 << 2);  // accel in low noise mode, gyro in low noise
+			static constexpr uint8_t off = 0x00;
 		};
 
 		static constexpr uint8_t FifoCount = 0x12;
@@ -119,22 +122,44 @@ struct ICM45Base {
 	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
 
 	void softResetIMU() {
-		i2c.writeReg(BaseRegs::DeviceConfig::reg, BaseRegs::DeviceConfig::valueSwReset);
+		m_RegisterInterface.writeReg(
+			BaseRegs::DeviceConfig::reg,
+			BaseRegs::DeviceConfig::valueSwReset
+		);
 		delay(35);
 	}
 
 	bool initializeBase() {
 		// perform initialization step
-		i2c.writeReg(BaseRegs::GyroConfig::reg, BaseRegs::GyroConfig::value);
-		i2c.writeReg(BaseRegs::AccelConfig::reg, BaseRegs::AccelConfig::value);
-		i2c.writeReg(BaseRegs::FifoConfig0::reg, BaseRegs::FifoConfig0::value);
-		i2c.writeReg(BaseRegs::FifoConfig3::reg, BaseRegs::FifoConfig3::value);
-		i2c.writeReg(BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::value);
+		m_RegisterInterface.writeReg(
+			BaseRegs::GyroConfig::reg,
+			BaseRegs::GyroConfig::value
+		);
+		m_RegisterInterface.writeReg(
+			BaseRegs::AccelConfig::reg,
+			BaseRegs::AccelConfig::value
+		);
+		m_RegisterInterface.writeReg(
+			BaseRegs::FifoConfig0::reg,
+			BaseRegs::FifoConfig0::value
+		);
+		m_RegisterInterface.writeReg(
+			BaseRegs::FifoConfig3::reg,
+			BaseRegs::FifoConfig3::value
+		);
+		m_RegisterInterface.writeReg(
+			BaseRegs::PwrMgmt0::reg,
+			BaseRegs::PwrMgmt0::value
+		);
 		delay(1);
 
 		return true;
 	}
 
+	void deinitialize() {
+		m_RegisterInterface.writeReg(BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::off);
+	}
+
 	template <typename AccelCall, typename GyroCall, typename TempCall>
 	void bulkRead(
 		AccelCall&& processAccelSample,
@@ -143,33 +168,12 @@ struct ICM45Base {
 	) {
 		// Allocate statically so that it does not take up stack space, which
 		// can result in stack overflow and panic
-		constexpr size_t MaxReadings = 8;
+		constexpr size_t MaxReadings = 6;
 		static std::array<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
 
 		constexpr int16_t InvalidReading = -32768;
 
-		size_t fifo_packets = i2c.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;
-		}
+		size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount);
 
 		if (fifo_packets == 0) {
 			return;
@@ -178,7 +182,8 @@ struct ICM45Base {
 		fifo_packets = std::min(fifo_packets, MaxReadings);
 
 		size_t bytes_to_read = fifo_packets * FullFifoEntrySize;
-		i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
+		m_RegisterInterface
+			.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
 
 		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
 			uint8_t header = read_buffer[i];

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

@@ -27,16 +27,20 @@
 #include <array>
 #include <cstdint>
 
+#include "../../../sensorinterface/RegisterInterface.h"
+
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
-template <typename I2CImpl>
 struct LSM6DSOutputHandler {
-	LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: i2c(i2c)
-		, logger(logger) {}
+	LSM6DSOutputHandler(
+		RegisterInterface& registerInterface,
+		SlimeVR::Logging::Logger& logger
+	)
+		: m_RegisterInterface(registerInterface)
+		, m_Logger(logger) {}
 
-	I2CImpl i2c;
-	SlimeVR::Logging::Logger& logger;
+	RegisterInterface& m_RegisterInterface;
+	SlimeVR::Logging::Logger& m_Logger;
 
 #pragma pack(push, 1)
 	struct FifoEntryAligned {
@@ -61,12 +65,13 @@ struct LSM6DSOutputHandler {
 		constexpr auto FIFO_SAMPLES_MASK = 0x3ff;
 		constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800;
 
-		const auto fifo_status = i2c.readReg16(Regs::FifoStatus);
+		const auto fifo_status = m_RegisterInterface.readReg16(Regs::FifoStatus);
 		const auto available_axes = fifo_status & FIFO_SAMPLES_MASK;
 		const auto fifo_bytes = available_axes * FullFifoEntrySize;
 		if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) {
 			// FIFO overrun is expected to happen during startup and calibration
-			logger.error("FIFO OVERRUN! This occuring during normal usage is an issue."
+			m_Logger.error(
+				"FIFO OVERRUN! This occuring during normal usage is an issue."
 			);
 		}
 
@@ -76,7 +81,8 @@ struct LSM6DSOutputHandler {
 									   static_cast<size_t>(fifo_bytes)
 								   )
 								 / FullFifoEntrySize * FullFifoEntrySize;
-		i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+		m_RegisterInterface
+			.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
 		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
 			FifoEntryAligned entry;
 			uint8_t tag = read_buffer[i] >> 3;

src/sensors/softfusion/drivers/lsm6ds3trc.h

@@ -27,6 +27,7 @@
 #include <array>
 #include <cstdint>
 
+#include "../../../sensorinterface/RegisterInterface.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -35,7 +36,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 416Hz, accel ODR = 416Hz
 
-template <typename I2CImpl>
 struct LSM6DS3TRC {
 	static constexpr uint8_t Address = 0x6a;
 	static constexpr auto Name = "LSM6DS3TR-C";
@@ -64,11 +64,11 @@ struct LSM6DS3TRC {
 		.restThAcc = 0.392f,
 	};
 
-	I2CImpl i2c;
-	SlimeVR::Logging::Logger logger;
-	LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: i2c(i2c)
-		, logger(logger) {}
+	RegisterInterface& m_RegisterInterface;
+	SlimeVR::Logging::Logger m_Logger;
+	LSM6DS3TRC(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: m_RegisterInterface(registerInterface)
+		, m_Logger(logger) {}
 
 	struct Regs {
 		struct WhoAmI {
@@ -111,14 +111,14 @@ struct LSM6DS3TRC {
 
 	bool initialize() {
 		// perform initialization step
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
 		delay(20);
-		i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
-		i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value);
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-		i2c.writeReg(Regs::FifoCtrl2::reg, Regs::FifoCtrl2::value);
-		i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value);
-		i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		m_RegisterInterface.writeReg(Regs::FifoCtrl2::reg, Regs::FifoCtrl2::value);
+		m_RegisterInterface.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value);
+		m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
 		return true;
 	}
 
@@ -128,12 +128,12 @@ struct LSM6DS3TRC {
 		GyroCall&& processGyroSample,
 		TempCall&& processTemperatureSample
 	) {
-		const auto read_result = i2c.readReg16(Regs::FifoStatus);
+		const auto read_result = m_RegisterInterface.readReg16(Regs::FifoStatus);
 		if (read_result & 0x4000) {  // overrun!
 			// disable and re-enable fifo to clear it
-			logger.debug("Fifo overrun, resetting...");
-			i2c.writeReg(Regs::FifoCtrl5::reg, 0);
-			i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
+			m_Logger.debug("Fifo overrun, resetting...");
+			m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, 0);
+			m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
 			return;
 		}
 		const auto unread_entries = read_result & 0x7ff;
@@ -150,7 +150,7 @@ struct LSM6DS3TRC {
 								 * sizeof(uint16_t) / single_measurement_bytes
 								 * single_measurement_bytes;
 
-		i2c.readBytes(
+		m_RegisterInterface.readBytes(
 			Regs::FifoData,
 			bytes_to_read,
 			reinterpret_cast<uint8_t*>(read_buffer.data())

src/sensors/softfusion/drivers/lsm6dso.h

@@ -36,8 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 416Hz, accel ODR = 104Hz
 
-template <typename I2CImpl>
-struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
+struct LSM6DSO : LSM6DSOutputHandler {
 	static constexpr uint8_t Address = 0x6a;
 	static constexpr auto Name = "LSM6DSO";
 	static constexpr auto Type = SensorTypeID::LSM6DSO;
@@ -68,8 +67,6 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
 		.restThAcc = 0.192f,
 	};
 
-	using LSM6DSOutputHandler<I2CImpl>::i2c;
-
 	struct Regs {
 		struct WhoAmI {
 			static constexpr uint8_t reg = 0x0f;
@@ -104,18 +101,24 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
 		static constexpr uint8_t FifoData = 0x78;
 	};
 
-	LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
+	LSM6DSO(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: LSM6DSOutputHandler(registerInterface, logger) {}
 
 	bool initialize() {
 		// perform initialization step
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
 		delay(20);
-		i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
-		i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-		i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
-		i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		m_RegisterInterface.writeReg(
+			Regs::FifoCtrl3BDR::reg,
+			Regs::FifoCtrl3BDR::value
+		);
+		m_RegisterInterface.writeReg(
+			Regs::FifoCtrl4Mode::reg,
+			Regs::FifoCtrl4Mode::value
+		);
 		return true;
 	}
 
@@ -125,7 +128,7 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
 		GyroCall&& processGyroSample,
 		TempCall&& processTempSample
 	) {
-		LSM6DSOutputHandler<I2CImpl>::
+		LSM6DSOutputHandler::
 			template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
 				processAccelSample,
 				processGyroSample,

src/sensors/softfusion/drivers/lsm6dsr.h

@@ -35,8 +35,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 416Hz, accel ODR = 104Hz
 
-template <typename I2CImpl>
-struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
+struct LSM6DSR : LSM6DSOutputHandler {
 	static constexpr uint8_t Address = 0x6a;
 	static constexpr auto Name = "LSM6DSR";
 	static constexpr auto Type = SensorTypeID::LSM6DSR;
@@ -67,8 +66,6 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
 		.restThAcc = 0.192f,
 	};
 
-	using LSM6DSOutputHandler<I2CImpl>::i2c;
-
 	struct Regs {
 		struct WhoAmI {
 			static constexpr uint8_t reg = 0x0f;
@@ -103,18 +100,24 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
 		static constexpr uint8_t FifoData = 0x78;
 	};
 
-	LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
+	LSM6DSR(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: LSM6DSOutputHandler(registerInterface, logger) {}
 
 	bool initialize() {
 		// perform initialization step
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
 		delay(20);
-		i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
-		i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-		i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
-		i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		m_RegisterInterface.writeReg(
+			Regs::FifoCtrl3BDR::reg,
+			Regs::FifoCtrl3BDR::value
+		);
+		m_RegisterInterface.writeReg(
+			Regs::FifoCtrl4Mode::reg,
+			Regs::FifoCtrl4Mode::value
+		);
 		return true;
 	}
 
@@ -124,15 +127,14 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
 		GyroCall&& processGyroSample,
 		TempCall&& processTempSample
 	) {
-		LSM6DSOutputHandler<I2CImpl>::
-			template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-				processAccelSample,
-				processGyroSample,
-				processTempSample,
-				GyrTs,
-				AccTs,
-				TempTs
-			);
+		LSM6DSOutputHandler::template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
+			processAccelSample,
+			processGyroSample,
+			processTempSample,
+			GyrTs,
+			AccTs,
+			TempTs
+		);
 	}
 };
 

src/sensors/softfusion/drivers/lsm6dsv.h

@@ -36,8 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 1000dps
 // Gyroscope ODR = 480Hz, accel ODR = 120Hz
 
-template <typename I2CImpl>
-struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
+struct LSM6DSV : LSM6DSOutputHandler {
 	static constexpr uint8_t Address = 0x6a;
 	static constexpr auto Name = "LSM6DSV";
 	static constexpr auto Type = SensorTypeID::LSM6DSV;
@@ -68,8 +67,6 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
 		.restThAcc = 0.192f,
 	};
 
-	using LSM6DSOutputHandler<I2CImpl>::i2c;
-
 	struct Regs {
 		struct WhoAmI {
 			static constexpr uint8_t reg = 0x0f;
@@ -116,21 +113,27 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
 		static constexpr uint8_t FifoData = 0x78;
 	};
 
-	LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: LSM6DSOutputHandler<I2CImpl>(i2c, logger) {}
+	LSM6DSV(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger)
+		: LSM6DSOutputHandler(registerInterface, logger) {}
 
 	bool initialize() {
 		// perform initialization step
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset);
 		delay(20);
-		i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value);
-		i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value);
-		i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value);
-		i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
-		i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value);
-		i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value);
-		i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value);
-		i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value);
+		m_RegisterInterface.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value);
+		m_RegisterInterface.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value);
+		m_RegisterInterface.writeReg(
+			Regs::FifoCtrl3BDR::reg,
+			Regs::FifoCtrl3BDR::value
+		);
+		m_RegisterInterface.writeReg(
+			Regs::FifoCtrl4Mode::reg,
+			Regs::FifoCtrl4Mode::value
+		);
 		return true;
 	}
 
@@ -140,15 +143,14 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
 		GyroCall&& processGyroSample,
 		TempCall&& processTempSample
 	) {
-		LSM6DSOutputHandler<I2CImpl>::
-			template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
-				processAccelSample,
-				processGyroSample,
-				processTempSample,
-				GyrTs,
-				AccTs,
-				TempTs
-			);
+		LSM6DSOutputHandler::bulkRead<AccelCall, GyroCall, TempCall, Regs>(
+			processAccelSample,
+			processGyroSample,
+			processTempSample,
+			GyrTs,
+			AccTs,
+			TempTs
+		);
 	}
 };
 

src/sensors/softfusion/drivers/mpu6050.h

@@ -29,6 +29,7 @@
 #include <array>
 #include <cstdint>
 
+#include "../../../sensorinterface/RegisterInterface.h"
 #include "vqf.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -37,7 +38,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
 // and gyroscope range at 1000dps
 // Gyroscope ODR = accel ODR = 250Hz
 
-template <typename I2CImpl>
 struct MPU6050 {
 	struct FifoSample {
 		uint8_t accel_x_h, accel_x_l;
@@ -77,11 +77,11 @@ struct MPU6050 {
 		.restThAcc = 0.784f,
 	};
 
-	I2CImpl i2c;
-	SlimeVR::Logging::Logger& logger;
-	MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
-		: i2c(i2c)
-		, logger(logger) {}
+	RegisterInterface& m_RegisterInterface;
+	SlimeVR::Logging::Logger& m_Logger;
+	MPU6050(RegisterInterface& i2c, SlimeVR::Logging::Logger& logger)
+		: m_RegisterInterface(i2c)
+		, m_Logger(logger) {}
 
 	struct Regs {
 		struct WhoAmI {
@@ -120,49 +120,52 @@ struct MPU6050 {
 	}
 
 	void resetFIFO() {
-		i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue);
+		m_RegisterInterface.writeReg(
+			Regs::UserCtrl::reg,
+			Regs::UserCtrl::fifoResetValue
+		);
 	}
 
 	bool initialize() {
 		// Reset
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_PWR_MGMT_1,
 			0x80
 		);  // PWR_MGMT_1: reset with 100ms delay (also disables sleep)
 		delay(100);
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_SIGNAL_PATH_RESET,
 			0x07
 		);  // full SIGNAL_PATH_RESET: with another 100ms delay
 		delay(100);
 
 		// Configure
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_PWR_MGMT_1,
 			0x01
 		);  // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_USER_CTRL,
 			0x00
 		);  // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_INT_ENABLE,
 			0x10
 		);  // 0001 0000 INT_ENABLE: only FIFO overflow interrupt
-		i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
-		i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
+		m_RegisterInterface.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_CONFIG,
 			0x02
 		);  // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro
 			// output rate to 1KHz)
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_SMPLRT_DIV,
 			0x03
 		);  // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate
 			// = Gyroscope Output Rate / (1 + SMPLRT_DIV))
 
-		i2c.writeReg(
+		m_RegisterInterface.writeReg(
 			MPU6050_RA_FIFO_EN,
 			0x78
 		);  // 0111 1000 FIFO_EN: All gyro axes + Accel
@@ -173,26 +176,30 @@ struct MPU6050 {
 	}
 
 	float getDirectTemp() const {
-		auto value = byteSwap(i2c.readReg16(Regs::OutTemp));
+		auto value = byteSwap(m_RegisterInterface.readReg16(Regs::OutTemp));
 		float result = (static_cast<int16_t>(value) / 340.0f) + 36.53f;
 		return result;
 	}
 
-	template <typename AccelCall, typename GyroCall>
-	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
-		const auto status = i2c.readReg(Regs::IntStatus);
+	template <typename AccelCall, typename GyroCall, typename TempCall>
+	void bulkRead(
+		AccelCall&& processAccelSample,
+		GyroCall&& processGyroSample,
+		TempCall&& processTempSample
+	) {
+		const auto status = m_RegisterInterface.readReg(Regs::IntStatus);
 
 		if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) {
 			// Overflows make it so we lose track of which packet is which
 			// This necessitates a reset
-			logger.debug("Fifo overrun, resetting...");
+			m_Logger.debug("Fifo overrun, resetting...");
 			resetFIFO();
 			return;
 		}
 
 		std::array<uint8_t, 12 * 10>
 			readBuffer;  // max 10 packages of 12byte values (sample) of data form fifo
-		auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount));
+		auto byteCount = byteSwap(m_RegisterInterface.readReg16(Regs::FifoCount));
 
 		auto readBytes = min(static_cast<size_t>(byteCount), readBuffer.size())
 					   / sizeof(FifoSample) * sizeof(FifoSample);
@@ -200,7 +207,7 @@ struct MPU6050 {
 			return;
 		}
 
-		i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data());
+		m_RegisterInterface.readBytes(Regs::FifoData, readBytes, readBuffer.data());
 		for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) {
 			const FifoSample* sample = reinterpret_cast<FifoSample*>(&readBuffer[i]);
 

src/sensors/softfusion/softfusionsensor.h

@@ -27,6 +27,7 @@
 #include <cstdlib>
 #include <tuple>
 
+#include "../../sensorinterface/i2cimpl.h"
 #include "../RestCalibrationDetector.h"
 #include "../SensorFusionRestDetect.h"
 #include "../sensor.h"
@@ -37,36 +38,35 @@
 namespace SlimeVR::Sensors {
 
 template <
-	template <typename I2CImpl>
 	typename T,
-	typename I2CImpl,
 	template <typename IMU, typename RawSensorT, typename RawVectorT>
 	typename Calibrator>
 class SoftFusionSensor : public Sensor {
-	using imu = T<I2CImpl>;
+	using SensorType = T;
 
 	static constexpr sensor_real_t getDefaultTempTs() {
 		if constexpr (DirectTempReadOnly) {
 			return DirectTempReadTs;
 		} else {
-			return imu::TempTs;
+			return SensorType::TempTs;
 		}
 	}
 
 	static constexpr bool Uses32BitSensorData
-		= requires(imu& i) { i.Uses32BitSensorData; };
+		= requires(SensorType& i) { i.Uses32BitSensorData; };
 
-	static constexpr bool DirectTempReadOnly = requires(imu& i) { i.getDirectTemp(); };
+	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. / imu::GyroSensitivity) / 32768.) * (PI / 180.0);
-	static constexpr float AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity;
+		= ((32768. / SensorType::GyroSensitivity) / 32768.) * (PI / 180.0);
+	static constexpr float AScale = CONST_EARTH_GRAVITY / SensorType::AccelSensitivity;
 
-	using Calib = Calibrator<imu, RawSensorT, RawVectorT>;
+	using Calib = Calibrator<SensorType, RawSensorT, RawVectorT>;
 
 	static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration;
 
@@ -76,12 +76,13 @@ class SoftFusionSensor : public Sensor {
 	uint32_t lastTempPollTime = micros();
 
 	bool detected() const {
-		const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg);
-		if (imu::Regs::WhoAmI::value != value) {
+		const auto value
+			= m_sensor.m_RegisterInterface.readReg(SensorType::Regs::WhoAmI::reg);
+		if (SensorType::Regs::WhoAmI::value != value) {
 			m_Logger.error(
 				"Sensor not detected, expected reg 0x%02x = 0x%02x but got 0x%02x",
-				imu::Regs::WhoAmI::reg,
-				imu::Regs::WhoAmI::value,
+				SensorType::Regs::WhoAmI::reg,
+				SensorType::Regs::WhoAmI::value,
 				value
 			);
 			return false;
@@ -122,7 +123,7 @@ class SoftFusionSensor : public Sensor {
 
 		m_fusion.updateAcc(accelData, calibrator.getAccelTimestep());
 
-		calibrator.provideAccelSample(xyz);
+		// calibrator.provideAccelSample(xyz);
 	}
 
 	void processGyroSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) {
@@ -133,15 +134,16 @@ class SoftFusionSensor : public Sensor {
 		calibrator.scaleGyroSample(gyroData);
 		m_fusion.updateGyro(gyroData, calibrator.getGyroTimestep());
 
-		calibrator.provideGyroSample(xyz);
+		// calibrator.provideGyroSample(xyz);
 	}
 
 	void
 	processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) {
 		if constexpr (!DirectTempReadOnly) {
-			const float scaledTemperature = imu::TemperatureBias
-										  + static_cast<float>(rawTemperature)
-												* (1.0 / imu::TemperatureSensitivity);
+			const float scaledTemperature
+				= SensorType::TemperatureBias
+				+ static_cast<float>(rawTemperature)
+					  * (1.0 / SensorType::TemperatureSensitivity);
 
 			lastReadTemperature = scaledTemperature;
 			if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) {
@@ -151,7 +153,7 @@ class SoftFusionSensor : public Sensor {
 				);
 			}
 
-			calibrator.provideTempSample(lastReadTemperature);
+			// calibrator.provideTempSample(lastReadTemperature);
 		}
 	}
 
@@ -204,20 +206,32 @@ class SoftFusionSensor : public Sensor {
 	}
 
 public:
-	static constexpr auto TypeID = imu::Type;
-	static constexpr uint8_t Address = imu::Address;
+	static constexpr auto TypeID = SensorType::Type;
+	static constexpr uint8_t Address = SensorType::Address;
 
 	SoftFusionSensor(
 		uint8_t id,
-		uint8_t i2cAddress,
+		RegisterInterface& registerInterface,
 		float rotation,
-		SlimeVR::SensorInterface* sensorInterface,
+		SlimeVR::SensorInterface* sensorInterface = nullptr,
 		PinInterface* intPin = nullptr,
 		uint8_t = 0
 	)
-		: Sensor(imu::Name, imu::Type, id, i2cAddress, rotation, sensorInterface)
-		, m_fusion(imu::SensorVQFParams, imu::GyrTs, imu::AccTs, imu::MagTs)
-		, m_sensor(I2CImpl(i2cAddress), m_Logger) {}
+		: Sensor(
+			SensorType::Name,
+			SensorType::Type,
+			id,
+			registerInterface,
+			rotation,
+			sensorInterface
+		)
+		, m_fusion(
+			  SensorType::SensorVQFParams,
+			  SensorType::GyrTs,
+			  SensorType::AccTs,
+			  SensorType::MagTs
+		  )
+		, m_sensor(registerInterface, m_Logger) {}
 	~SoftFusionSensor() override = default;
 
 	void checkSensorTimeout() {
@@ -241,7 +255,7 @@ public:
 	}
 
 	void motionLoop() final {
-		calibrator.tick();
+		// calibrator.tick();
 
 		// read fifo updating fusion
 		uint32_t now = micros();
@@ -254,7 +268,7 @@ public:
 					- (tempElapsed - static_cast<uint32_t>(DirectTempReadTs * 1e6));
 				lastReadTemperature = m_sensor.getDirectTemp();
 
-				calibrator.provideTempSample(lastReadTemperature);
+				// calibrator.provideTempSample(lastReadTemperature);
 
 				if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) {
 					tempGradientCalculator.feedSample(
@@ -314,7 +328,7 @@ public:
 
 	void motionSetup() final {
 		if (!detected()) {
-			m_status = SensorStatus::SENSOR_ERROR;
+			m_status = SensorStatus::SENSOR_OFFLINE;
 			return;
 		}
 
@@ -323,14 +337,14 @@ public:
 
 		toggles = configuration.getSensorToggles(sensorId);
 
+		/*
 		// If no compatible calibration data is found, the calibration data will just be
 		// zero-ed out
 		if (calibrator.calibrationMatches(sensorCalibration)) {
 			calibrator.assignCalibration(sensorCalibration);
-		} else if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::NONE) {
-			m_Logger.warn(
-				"No calibration data found for sensor %d, ignoring...",
-				sensorId
+		} else if (sensorCalibration.type ==
+		SlimeVR::Configuration::SensorConfigType::NONE) { m_Logger.warn( "No calibration
+		data found for sensor %d, ignoring...", sensorId
 			);
 			m_Logger.info("Calibration is advised");
 		} else {
@@ -340,13 +354,14 @@ public:
 			);
 			m_Logger.info("Please recalibrate");
 		}
+		*/
 
 		calibrator.begin();
 
 		bool initResult = false;
 
 		if constexpr (Calib::HasMotionlessCalib) {
-			typename imu::MotionlessCalibrationData calibData;
+			typename SensorType::MotionlessCalibrationData calibData;
 			std::memcpy(
 				&calibData,
 				calibrator.getMotionlessCalibrationData(),
@@ -394,11 +409,11 @@ public:
 	}
 
 	void startCalibration(int calibrationType) final {
-		calibrator.startCalibration(
-			calibrationType,
-			[&](const uint32_t seconds) { eatSamplesForSeconds(seconds); },
-			[&](const uint32_t millis) { return eatSamplesReturnLast(millis); }
-		);
+		// calibrator.startCalibration(
+		// 	calibrationType,
+		// 	[&](const uint32_t seconds) { eatSamplesForSeconds(seconds); },
+		// 	[&](const uint32_t millis) { return eatSamplesReturnLast(millis); }
+		// );
 	}
 
 	bool isFlagSupported(SensorToggles toggle) const final {
@@ -408,8 +423,14 @@ public:
 
 	SensorStatus getSensorState() final { return m_status; }
 
+	void deinitialize() override final {
+		if constexpr (requires { m_sensor.deinitialize(); }) {
+			m_sensor.deinitialize();
+		}
+	}
+
 	SensorFusionRestDetect m_fusion;
-	imu m_sensor;
+	SensorType m_sensor;
 	Calib calibrator{
 		m_fusion,
 		m_sensor,
@@ -418,8 +439,7 @@ public:
 		getDefaultTempTs(),
 		AScale,
 		GScale,
-		toggles
-	};
+		toggles};
 
 	SensorStatus m_status = SensorStatus::SENSOR_OFFLINE;
 	uint32_t m_lastPollTime = micros();
@@ -428,6 +448,25 @@ public:
 	uint32_t m_lastTemperaturePacketSent = 0;
 
 	RestCalibrationDetector calibrationDetector;
+
+	static bool checkPresent(uint8_t sensorID, const RegisterInterface& imuInterface) {
+		I2Cdev::readTimeout = 100;
+		auto value = imuInterface.readReg(SensorType::Regs::WhoAmI::reg);
+		I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
+		if (SensorType::Regs::WhoAmI::value == value) {
+			return true;
+		}
+		return false;
+	}
+
+	static bool checkPresent(uint8_t sensorID, uint8_t imuAddress) {
+		uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID;
+		if (!I2CSCAN::hasDevOnBus(address)) {
+			return false;
+		}
+		const I2CImpl& i2c = I2CImpl(address);
+		return checkPresent(sensorID, i2c);
+	}
 };
 
 }  // namespace SlimeVR::Sensors