Cleanup

SlimeVR · Jan 5, 2025 · cee6961 · cee6961
1 parent ae66451
commit cee6961
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 28 deletions.
diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h
@@ -25,6 +25,7 @@
 
 #include <cstring>
 #include <functional>
+#include <tuple>
 #include <vector>
 
 #include "GlobalVars.h"
@@ -44,6 +45,8 @@ template <
 	typename RawVectorT>
 class SoftfusionCalibrator {
 public:
+	static constexpr bool HasUpsideDownCalibration = true;
+
 	static constexpr bool HasMotionlessCalib
 		= requires(IMU& i) { typename IMU::MotionlessCalibrationData; };
 	static constexpr size_t MotionlessCalibDataSize() {
@@ -54,6 +57,11 @@ class SoftfusionCalibrator {
 		}
 	}
 
+	using EatSamplesFn = std::function<void(const uint32_t)>;
+	using ReturnLastFn
+		= std::function<std::tuple<RawVectorT, RawVectorT, int16_t>(const uint32_t)>;
+	using RecalcFusionFn = std::function<void()>;
+
 	SoftfusionCalibrator(
 		IMU& sensor,
 		uint8_t sensorId,
@@ -69,10 +77,9 @@ class SoftfusionCalibrator {
 
 	void startCalibration(
 		int calibrationType,
-		const std::function<void(const uint32_t)>& eatSamplesForSeconds,
-		const std::function<std::tuple<RawVectorT, RawVectorT, int16_t>(const uint32_t
-		)>& eatSamplesReturnLast,
-		const std::function<void()>& recalcFusion
+		const EatSamplesFn& eatSamplesForSeconds,
+		const ReturnLastFn& eatSamplesReturnLast,
+		const RecalcFusionFn& recalcFusion
 	) {
 		if (calibrationType == 0) {
 			// ALL
@@ -184,10 +191,7 @@ class SoftfusionCalibrator {
 		configuration.save();
 	}
 
-	void calibrateGyroOffset(
-		const std::function<std::tuple<RawVectorT, RawVectorT, int16_t>(const uint32_t
-		)>& eatSamplesReturnLast
-	) {
+	void calibrateGyroOffset(const ReturnLastFn& eatSamplesReturnLast) {
 		// Wait for sensor to calm down before calibration
 		m_Logger.info(
 			"Put down the device and wait for baseline gyro reading calibration (%d "
@@ -243,8 +247,7 @@ class SoftfusionCalibrator {
 		);
 	}
 
-	void calibrateAccel(const std::function<void(const uint32_t)>& eatSamplesForSeconds
-	) {
+	void calibrateAccel(const EatSamplesFn& eatSamplesForSeconds) {
 		auto magneto = std::make_unique<MagnetoCalibration>();
 		m_Logger.info(
 			"Put the device into 6 unique orientations (all sides), leave it still and "
@@ -373,8 +376,8 @@ class SoftfusionCalibrator {
 	}
 
 	void calibrateSampleRate(
-		const std::function<void(const uint32_t)>& eatSamplesForSeconds,
-		const std::function<void()>& recalcFusion
+		const EatSamplesFn& eatSamplesForSeconds,
+		const RecalcFusionFn& recalcFusion
 	) {
 		m_Logger.debug(
 			"Calibrating IMU sample rate in %d second(s)...",

diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h
@@ -61,24 +61,14 @@ class SoftFusionSensor : public Sensor {
 		typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
 	using RawVectorT = std::array<RawSensorT, 3>;
 
-	static constexpr auto UpsideDownCalibrationInit = true;
-
 	static constexpr double GScale
 		= ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0);
 	static constexpr double AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity;
 
 	using Calib
 		= Calibrator<imu, getDefaultTempTs(), AScale, GScale, RawSensorT, RawVectorT>;
 
-	static constexpr bool HasMotionlessCalib
-		= requires(imu& i) { typename imu::MotionlessCalibrationData; };
-	static constexpr size_t MotionlessCalibDataSize() {
-		if constexpr (HasMotionlessCalib) {
-			return sizeof(typename imu::MotionlessCalibrationData);
-		} else {
-			return 0;
-		}
-	}
+	static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration;
 
 	static constexpr float DirectTempReadFreq = 15;
 	static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq;
@@ -118,13 +108,11 @@ class SoftFusionSensor : public Sensor {
 	}};
 
 	void recalcFusion() {
-		auto& calibration = calibrator.getCalibration();
-
 		m_fusion = SensorFusionRestDetect(
 			imu::SensorVQFParams,
-			calibration.G_Ts,
-			calibration.A_Ts,
-			calibration.M_Ts
+			calibrator.getGyroTimestep(),
+			calibrator.getAccelTimestep(),
+			calibrator.getTempTimestep()
 		);
 	}