Mag support (Attempt 2)

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/drivers/icm45base.h

@@ -26,6 +26,7 @@
 
 #include "../../../sensorinterface/RegisterInterface.h"
 #include "callbacks.h"
+#include "sensors/softfusion/magdriver.h"
 
 namespace SlimeVR::Sensors::SoftFusion::Drivers {
 
@@ -104,6 +105,77 @@ struct ICM45Base {
 
 		static constexpr uint8_t FifoCount = 0x12;
 		static constexpr uint8_t FifoData = 0x14;
+
+		// Indirect Register Access
+
+		static constexpr uint32_t IRegWaitTimeMicros = 4;
+
+		enum class Bank : uint8_t {
+			IMemSram = 0x00,
+			IPregBar = 0xa0,
+			IPregSys1 = 0xa4,
+			IPregSys2 = 0xa5,
+			IPregTop1 = 0xa2,
+		};
+
+		static constexpr uint8_t IRegAddr = 0x7c;
+		static constexpr uint8_t IRegData = 0x7e;
+
+		// Mag Support
+
+		struct IOCPadScenarioAuxOvrd {
+			static constexpr uint8_t reg = 0x30;
+			static constexpr uint8_t value = (0b1 << 4)  // Enable AUX1 override
+										   | (0b01 << 2)  // Enable I2CM master
+										   | (0b1 << 1)  // Enable AUX1 enable override
+										   | (0b1 << 0);  // Enable AUX1
+		};
+
+		struct I2CMCommand0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x06;
+		};
+
+		struct I2CMDevProfile0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x0e;
+		};
+
+		struct I2CMDevProfile1 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x0f;
+		};
+
+		struct I2CMWrData0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x33;
+		};
+
+		struct I2CMRdData0 {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x1b;
+		};
+
+		struct DmpExtSenOdrCfg {
+			// TODO: todo
+		};
+
+		struct I2CMControl {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x16;
+		};
+
+		struct I2CMStatus {
+			static constexpr Bank bank = Bank::IPregTop1;
+			static constexpr uint8_t reg = 0x18;
+
+			static constexpr uint8_t SDAErr = 0b1 << 5;
+			static constexpr uint8_t SCLErr = 0b1 << 4;
+			static constexpr uint8_t SRSTErr = 0b1 << 3;
+			static constexpr uint8_t TimeoutErr = 0b1 << 2;
+			static constexpr uint8_t Done = 0b1 << 1;
+			static constexpr uint8_t Busy = 0b1 << 0;
+		};
 	};
 
 #pragma pack(push, 1)
@@ -149,6 +221,11 @@ struct ICM45Base {
 			BaseRegs::PwrMgmt0::value
 		);
 
+		m_RegisterInterface.writeReg(
+			BaseRegs::IOCPadScenarioAuxOvrd::reg,
+			BaseRegs::IOCPadScenarioAuxOvrd::value
+		);
+
 		read_buffer.resize(FullFifoEntrySize * MaxReadings);
 
 		delay(1);
@@ -231,6 +308,130 @@ struct ICM45Base {
 			}
 		}
 	}
+
+	template <typename Reg>
+	uint8_t readBankRegister() {
+		uint8_t buffer;
+		readBankRegister<Reg>(&buffer, sizeof(buffer));
+		return buffer;
+	}
+
+	template <typename Reg, typename T>
+	void readBankRegister(T* buffer, size_t length) {
+		uint8_t data[] = {
+			static_cast<uint8_t>(Reg::bank),
+			Reg::reg,
+		};
+
+		auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
+		m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
+		delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		for (size_t i = 0; i < length * sizeof(T); i++) {
+			bufferBytes[i] = m_RegisterInterface.readReg(BaseRegs::IRegData);
+			delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		}
+	}
+
+	template <typename Reg>
+	void writeBankRegister() {
+		writeBankRegister<Reg>(&Reg::value, sizeof(Reg::value));
+	}
+
+	template <typename Reg, typename T>
+	void writeBankRegister(T* buffer, size_t length) {
+		auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
+
+		uint8_t data[] = {
+			static_cast<uint8_t>(Reg::bank),
+			Reg::reg,
+			bufferBytes[0],
+		};
+
+		m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
+		delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		for (size_t i = 1; i < length * sizeof(T); i++) {
+			m_RegisterInterface.writeReg(BaseRegs::IRegData, bufferBytes[i]);
+			delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
+		}
+	}
+
+	template <typename Reg>
+	void writeBankRegister(uint8_t value) {
+		writeBankRegister<Reg>(&value, sizeof(value));
+	}
+
+	void setAuxId(uint8_t deviceId) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile1>(deviceId);
+	}
+
+	uint8_t readAux(uint8_t address) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
+
+		writeBankRegister<typename BaseRegs::I2CMCommand0>(
+			(0b1 << 7)  // Last transaction
+			| (0b0 << 6)  // Channel 0
+			| (0b01 << 4)  // Read with register
+			| (0b0001 << 0)  // Read 1 byte
+		);
+		writeBankRegister<typename BaseRegs::I2CMControl>(
+			(0b0 << 6)  // No restarts
+			| (0b0 << 3)  // Fast mode
+			| (0b1 << 0)  // Start transaction
+		);
+
+		uint8_t lastStatus;
+		while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
+			   & BaseRegs::I2CMStatus::Busy)
+			;
+
+		if (lastStatus != BaseRegs::I2CMStatus::Done) {
+			m_Logger.error(
+				"Aux read from address 0x%02x returned status 0x%02x",
+				address,
+				lastStatus
+			);
+		}
+
+		return readBankRegister<typename BaseRegs::I2CMRdData0>();
+	}
+
+	void writeAux(uint8_t address, uint8_t value) {
+		writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
+		writeBankRegister<typename BaseRegs::I2CMWrData0>(value);
+		writeBankRegister<typename BaseRegs::I2CMCommand0>(
+			(0b1 << 7)  // Last transaction
+			| (0b0 << 6)  // Channel 0
+			| (0b01 << 4)  // Read with register
+			| (0b0001 << 0)  // Read 1 byte
+		);
+		writeBankRegister<typename BaseRegs::I2CMControl>(
+			(0b0 << 6)  // No restarts
+			| (0b0 << 3)  // Fast mode
+			| (0b1 << 0)  // Start transaction
+		);
+
+		uint8_t lastStatus;
+		while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
+			   & BaseRegs::I2CMStatus::Busy)
+			;
+
+		if (lastStatus != BaseRegs::I2CMStatus::Done) {
+			m_Logger.error(
+				"Aux write to address 0x%02x with value 0x%02x returned status 0x%02x",
+				address,
+				value,
+				lastStatus
+			);
+		}
+	}
+
+	void startAuxPolling(uint8_t dataReg, MagDataWidth dataWidth) {
+		// TODO:
+	}
+
+	void stopAuxPolling() {
+		// TODO:
+	}
 };
 
 };  // namespace SlimeVR::Sensors::SoftFusion::Drivers

src/sensors/softfusion/imuconsts.h

@@ -56,4 +56,13 @@ struct IMUConsts {
 			return IMU::TempTs;
 		}
 	}
+
+	static constexpr bool SupportsMags = requires(IMU& i) { i.readAux(0x00); };
+	static constexpr bool Supports9ByteMag = []() constexpr {
+		if constexpr (requires { IMU::Supports9ByteMag; }) {
+			return IMU::Supports9ByteMag;
+		} else {
+			return true;
+		}
+	}();
 };