Make SPI work

src/debug.h

@@ -104,7 +104,7 @@
 #define USE_NONBLOCKING_CALIBRATION true
 #endif
 
-#define DEBUG_MEASURE_SENSOR_TIME_TAKEN true
+#define DEBUG_MEASURE_SENSOR_TIME_TAKEN false
 
 #ifndef DEBUG_MEASURE_SENSOR_TIME_TAKEN
 #define DEBUG_MEASURE_SENSOR_TIME_TAKEN false

src/defines.h

@@ -26,7 +26,7 @@
 // ================================================
 
 // Set parameters of IMU and board used
-#define IMU IMU_BNO085
+#define IMU IMU_ICM45686
 #define SECOND_IMU IMU
 #define BOARD BOARD_SLIMEVR
 #define IMU_ROTATION DEG_270
@@ -54,24 +54,25 @@ 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_SPI(                                          \
+		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 +240,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

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 @@ class Logger {
 
 	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/sensorinterface/RegisterInterface.h

@@ -0,0 +1,44 @@
+/*
+	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::SoftFusion {
+
+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;
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion

src/sensorinterface/SPIImpl.h

@@ -0,0 +1,156 @@
+/*
+	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::SoftFusion {
+
+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 {
+		static_assert("SPI doesn't have addresses, you're using incompatible sensors");
+		return 0;
+	}
+
+	operator uint8_t() const { return getAddress(); }
+
+	operator std::string() 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::SoftFusion