Softfusion cleanup

src/sensors/SensorBuilder.h

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

src/sensors/SensorToggles.cpp

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

src/sensors/SensorToggles.h

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

src/sensors/bno080sensor.cpp

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

src/sensors/sensor.cpp

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

src/sensors/sensor.h

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

src/sensors/softfusion/CalibrationBase.h

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