diff --git a/src/debug.h b/src/debug.h index 56e59175..06da9775 100644 --- a/src/debug.h +++ b/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 diff --git a/src/defines.h b/src/defines.h index f1c5ecc1..7b5223b8 100644 --- a/src/defines.h +++ b/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 diff --git a/src/logging/Logger.cpp b/src/logging/Logger.cpp index be8d68b9..4508369b 100644 --- a/src/logging/Logger.cpp +++ b/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; } diff --git a/src/logging/Logger.h b/src/logging/Logger.h index 09d1d2b9..14e69d80 100644 --- a/src/logging/Logger.h +++ b/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 - 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 - 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 - 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 - 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 - 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 - 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 - 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; } diff --git a/src/sensorinterface/RegisterInterface.h b/src/sensorinterface/RegisterInterface.h new file mode 100644 index 00000000..ffd75626 --- /dev/null +++ b/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 + +#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 diff --git a/src/sensorinterface/SPIImpl.h b/src/sensorinterface/SPIImpl.h new file mode 100644 index 00000000..1c3c6ff8 --- /dev/null +++ b/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 +#include + +#include + +#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 diff --git a/src/sensors/softfusion/i2cimpl.h b/src/sensorinterface/i2cimpl.h similarity index 77% rename from src/sensors/softfusion/i2cimpl.h rename to src/sensorinterface/i2cimpl.h index 80ac421d..7694c9a2 100644 --- a/src/sensors/softfusion/i2cimpl.h +++ b/src/sensorinterface/i2cimpl.h @@ -26,22 +26,21 @@ #include #include "I2Cdev.h" +#include "RegisterInterface.h" namespace SlimeVR::Sensors::SoftFusion { -struct I2CImpl { - static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2; - +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 +51,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,14 +64,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; } + + operator uint8_t() const { return getAddress(); } + + operator std::string() const { + return std::string("I2C"); + } + private: uint8_t m_devAddr; }; diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 4089ce76..ea346d42 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -46,7 +46,6 @@ #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 @@ -61,32 +60,33 @@ namespace SlimeVR { namespace Sensors { -using SoftFusionLSM6DS3TRC = SoftFusionSensor< - SoftFusion::Drivers::LSM6DS3TRC, - SoftFusion::I2CImpl, - SFCALIBRATOR>; -using SoftFusionICM42688 = SoftFusionSensor< - SoftFusion::Drivers::ICM42688, - SoftFusion::I2CImpl, - SFCALIBRATOR>; +template +using SoftFusionLSM6DS3TRC + = SoftFusionSensor; +template +using SoftFusionICM42688 + = SoftFusionSensor; +template using SoftFusionBMI270 - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionLSM6DSV - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionLSM6DSO - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionLSM6DSR - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionMPU6050 - = SoftFusionSensor; -using SoftFusionICM45686 = SoftFusionSensor< - SoftFusion::Drivers::ICM45686, - SoftFusion::I2CImpl, - SFCALIBRATOR>; -using SoftFusionICM45605 = SoftFusionSensor< - SoftFusion::Drivers::ICM45605, - SoftFusion::I2CImpl, - SFCALIBRATOR>; + = SoftFusionSensor; +template +using SoftFusionICM45686 + = SoftFusionSensor; +template +using SoftFusionICM45605 + = SoftFusionSensor; void SensorManager::setup() { std::map directPinInterfaces; @@ -134,20 +134,40 @@ void SensorManager::setup() { } #define NO_PIN nullptr +#define NO_WIRE new EmptySensorInterface #define DIRECT_PIN(pin) directPin(pin) #define DIRECT_WIRE(scl, sda) directWire(scl, sda) #define MCP_PIN(pin) mcpPin(pin) #define PCA_WIRE(scl, sda, addr, ch) pcaWire(scl, sda, addr, ch) +#define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ + SoftFusion::SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN) -#define SENSOR_DESC_ENTRY(ImuType, ...) \ - { \ - auto sensor = buildSensor(sensorID, __VA_ARGS__); \ - if (sensor->isWorking()) { \ - m_Logger.info("Sensor %d configured", sensorID + 1); \ - activeSensorCount++; \ - } \ - m_Sensors.push_back(std::move(sensor)); \ - sensorID++; \ +#define SENSOR_DESC_ENTRY(ImuType, ...) \ + { \ + auto sensor = buildSensor, SoftFusion::I2CImpl>( \ + sensorID, \ + __VA_ARGS__ \ + ); \ + if (sensor->isWorking()) { \ + m_Logger.info("Sensor %d configured", sensorID + 1); \ + activeSensorCount++; \ + } \ + m_Sensors.push_back(std::move(sensor)); \ + sensorID++; \ + } + +#define SENSOR_DESC_ENTRY_SPI(ImuType, ...) \ + { \ + auto sensor = buildSensor, SoftFusion::SPIImpl>( \ + 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 @@ -157,10 +177,6 @@ void SensorManager::setup() { { 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) { diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index b30982b4..0e7970b5 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -45,6 +45,8 @@ #include "sensorinterface/I2CPCAInterface.h" #include "sensorinterface/I2CWireSensorInterface.h" #include "sensorinterface/MCP23X17PinInterface.h" +#include "sensorinterface/SPIImpl.h" +#include "sensorinterface/i2cimpl.h" namespace SlimeVR { namespace Sensors { @@ -71,23 +73,25 @@ class SensorManager { std::vector> m_Sensors; Adafruit_MCP23X17 m_MCP; - template + template std::unique_ptr<::Sensor> buildSensor( uint8_t sensorID, - std::optional imuAddress, + std::optional imuInterface, float rotation, SensorInterface* sensorInterface, bool optional = false, PinInterface* intPin = nullptr, int extraParam = 0 ) { - uint8_t i2cAddress = imuAddress.value_or(ImuType::Address + sensorID); + RegInterface interface = imuInterface.value_or( + RegInterface(ImuType::Address + sensorID) + ); 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, + interface, rotation, sensorInterface, intPin, @@ -102,23 +106,19 @@ class SensorManager { sensorInterface->init(); sensorInterface->swapIn(); - if (I2CSCAN::hasDevOnBus(i2cAddress)) { - m_Logger - .trace("Sensor %d found at address 0x%02X", sensorID + 1, i2cAddress); + if (interface.hasSensorOnBus()) { + m_Logger.trace("Sensor %d found at address 0x%s", sensorID + 1, interface); } else { if (!optional) { - m_Logger.error( - "Mandatory sensor %d not found at address 0x%02X", - sensorID + 1, - i2cAddress + m_Logger + .error("Mandatory sensor %d not found at address 0x%s", sensorID + 1, interface); + sensor = std::make_unique( + sensorID, + ImuType::SensorTypeID ); - sensor = std::make_unique(sensorID, ImuType::TypeID); } else { - m_Logger.debug( - "Optional sensor %d not found at address 0x%02X", - sensorID + 1, - i2cAddress - ); + m_Logger + .debug("Optional sensor %d not found at address 0x%s", sensorID + 1, interface); sensor = std::make_unique(sensorID); } return sensor; @@ -126,7 +126,7 @@ class SensorManager { sensor = std::make_unique( sensorID, - i2cAddress, + interface, rotation, sensorInterface, intPin, diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index ab139c05..782e84e4 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -39,7 +39,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 400Hz, accel ODR = 100Hz // Timestamps reading are not used -template +template 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) {} + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + int8_t m_zxFactor; + BMI270(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) + , m_zxFactor(0) {} struct Regs { struct WhoAmI { @@ -244,14 +244,14 @@ 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 +261,57 @@ 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 + RegisterInterface::MaxTransactionLength ); - i2c.writeBytes( + m_RegisterInterface.writeBytes( Regs::InitData, burstWrite, const_cast(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(zx_factor_reg | sign_byte); + m_zxFactor = static_cast(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); } @@ -330,43 +330,43 @@ struct BMI270 { // 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 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 +395,11 @@ 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(i2c.readReg16(Regs::TempData)); + const auto value = static_cast(m_RegisterInterface.readReg16(Regs::TempData)); return static_cast(value) * TempStep + 23.0f; } - using FifoBuffer = std::array; + using FifoBuffer = std::array; FifoBuffer read_buffer; template @@ -416,13 +416,13 @@ 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(read_buffer.size()), static_cast(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(i, read_buffer); @@ -451,7 +451,7 @@ struct BMI270 { gyro[0] = std::clamp( static_cast(gyro[0]) - static_cast( - (static_cast(zxFactor) * gyro[2]) / 512 + (static_cast(m_zxFactor) * gyro[2]) / 512 ), static_cast(ShortLimit::min()), static_cast(ShortLimit::max()) diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index 76760725..a8d4e8b9 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -36,7 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 500Hz, accel ODR = 100Hz // Timestamps reading not used, as they're useless (constant predefined increment) -template +template struct ICM42688 { static constexpr uint8_t Address = 0x68; static constexpr auto Name = "ICM-42688"; @@ -66,11 +66,11 @@ struct ICM42688 { .restThAcc = 0.196f, }; - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + ICM42688(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} struct Regs { struct WhoAmI { @@ -143,15 +143,15 @@ 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 +163,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 read_buffer; // max 8 readings const auto bytes_to_read = std::min( @@ -171,7 +171,7 @@ struct ICM42688 { static_cast(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( diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h index fbc54686..8296c011 100644 --- a/src/sensors/softfusion/drivers/icm45605.h +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -35,8 +35,8 @@ 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 -struct ICM45605 : public ICM45Base { +template +struct ICM45605 : public ICM45Base { static constexpr auto Name = "ICM-45605"; static constexpr auto Type = SensorTypeID::ICM45605; @@ -48,8 +48,8 @@ struct ICM45605 : public ICM45Base { .restThAcc = 0.0098f, }; - ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : ICM45Base{i2c, logger} {} + ICM45605(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : ICM45Base{registerInterface, logger} {} struct Regs { struct WhoAmI { @@ -59,8 +59,8 @@ struct ICM45605 : public ICM45Base { }; bool initialize() { - ICM45Base::softResetIMU(); - return ICM45Base::initializeBase(); + ICM45Base::softResetIMU(); + return ICM45Base::initializeBase(); } }; diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h index 1306b8f9..efea8753 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -35,8 +35,8 @@ 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 -struct ICM45686 : public ICM45Base { +template +struct ICM45686 : public ICM45Base { static constexpr auto Name = "ICM-45686"; static constexpr auto Type = SensorTypeID::ICM45686; @@ -48,8 +48,8 @@ struct ICM45686 : public ICM45Base { .restThAcc = 0.196f, }; - ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : ICM45Base{i2c, logger} {} + ICM45686(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : ICM45Base{registerInterface, logger} {} struct Regs { struct WhoAmI { @@ -68,13 +68,13 @@ struct ICM45686 : public ICM45Base { }; }; - using ICM45Base::i2c; + using ICM45Base::m_RegisterInterface; bool initialize() { - ICM45Base::softResetIMU(); - i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); - i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value); - return ICM45Base::initializeBase(); + ICM45Base::softResetIMU(); + m_RegisterInterface.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); + m_RegisterInterface.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value); + return ICM45Base::initializeBase(); } }; diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 4243ed12..91eccdea 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -33,7 +33,7 @@ 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 +template struct ICM45Base { static constexpr uint8_t Address = 0x68; @@ -56,11 +56,11 @@ struct ICM45Base { static constexpr int fifoReadSize = 8; // Can't be more than 12 or it will overflow i2c read size, default 8 - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + ICM45Base(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} struct BaseRegs { static constexpr uint8_t TempData = 0x0c; @@ -127,17 +127,17 @@ struct ICM45Base { static constexpr size_t FullFifoBufferSize = sizeof(FifoBuffer); 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; @@ -149,7 +149,7 @@ struct ICM45Base { GyroCall&& processGyroSample, TempCall&& processTemperatureSample ) { - const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount); + const auto fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount); const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); FifoBuffer read_buffer; @@ -158,7 +158,7 @@ struct ICM45Base { static_cast(fifo_bytes) ) / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(BaseRegs::FifoData, bytes_to_read, (uint8_t*)&read_buffer); + m_RegisterInterface.readBytes(BaseRegs::FifoData, bytes_to_read, (uint8_t*)&read_buffer); uint8_t index = 0; for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { FifoEntryAligned entry = read_buffer.entry[index++]; diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 24c61773..57705338 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -29,14 +29,14 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { -template +template struct LSM6DSOutputHandler { - LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + LSM6DSOutputHandler(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; #pragma pack(push, 1) struct FifoEntryAligned { @@ -61,12 +61,12 @@ 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 +76,7 @@ struct LSM6DSOutputHandler { static_cast(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; diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index e2e44e6a..707838bc 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -35,7 +35,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 416Hz -template +template 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) {} + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger m_Logger; + LSM6DS3TRC(RegInterface 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(read_buffer.data()) diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 17d17bc2..05e3af6e 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -36,8 +36,8 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 104Hz -template -struct LSM6DSO : LSM6DSOutputHandler { +template +struct LSM6DSO : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSO"; static constexpr auto Type = SensorTypeID::LSM6DSO; @@ -68,7 +68,7 @@ struct LSM6DSO : LSM6DSOutputHandler { .restThAcc = 0.192f, }; - using LSM6DSOutputHandler::i2c; + using LSM6DSOutputHandler::m_RegisterInterface; struct Regs { struct WhoAmI { @@ -104,18 +104,18 @@ struct LSM6DSO : LSM6DSOutputHandler { static constexpr uint8_t FifoData = 0x78; }; - LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSO(RegInterface 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 +125,7 @@ struct LSM6DSO : LSM6DSOutputHandler { GyroCall&& processGyroSample, TempCall&& processTempSample ) { - LSM6DSOutputHandler:: + LSM6DSOutputHandler:: template bulkRead( processAccelSample, processGyroSample, diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index a9d7ac1d..0a5fe268 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -35,8 +35,8 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 104Hz -template -struct LSM6DSR : LSM6DSOutputHandler { +template +struct LSM6DSR : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSR"; static constexpr auto Type = SensorTypeID::LSM6DSR; @@ -67,7 +67,7 @@ struct LSM6DSR : LSM6DSOutputHandler { .restThAcc = 0.192f, }; - using LSM6DSOutputHandler::i2c; + using LSM6DSOutputHandler::m_RegisterInterface; struct Regs { struct WhoAmI { @@ -103,18 +103,18 @@ struct LSM6DSR : LSM6DSOutputHandler { static constexpr uint8_t FifoData = 0x78; }; - LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSR(RegInterface 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,7 +124,7 @@ struct LSM6DSR : LSM6DSOutputHandler { GyroCall&& processGyroSample, TempCall&& processTempSample ) { - LSM6DSOutputHandler:: + LSM6DSOutputHandler:: template bulkRead( processAccelSample, processGyroSample, diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index 60c9a9bf..1317549b 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -36,8 +36,8 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 480Hz, accel ODR = 120Hz -template -struct LSM6DSV : LSM6DSOutputHandler { +template +struct LSM6DSV : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSV"; static constexpr auto Type = SensorTypeID::LSM6DSV; @@ -68,7 +68,7 @@ struct LSM6DSV : LSM6DSOutputHandler { .restThAcc = 0.192f, }; - using LSM6DSOutputHandler::i2c; + using LSM6DSOutputHandler::m_RegisterInterface; struct Regs { struct WhoAmI { @@ -116,21 +116,21 @@ struct LSM6DSV : LSM6DSOutputHandler { static constexpr uint8_t FifoData = 0x78; }; - LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSV(RegInterface 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,7 +140,7 @@ struct LSM6DSV : LSM6DSOutputHandler { GyroCall&& processGyroSample, TempCall&& processTempSample ) { - LSM6DSOutputHandler:: + LSM6DSOutputHandler:: template bulkRead( processAccelSample, processGyroSample, diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 79c24ba6..aa58d995 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -37,7 +37,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = accel ODR = 250Hz -template +template 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) {} + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + MPU6050(RegInterface i2c, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(i2c) + , m_Logger(logger) {} struct Regs { struct WhoAmI { @@ -120,49 +120,49 @@ 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 +173,26 @@ struct MPU6050 { } float getDirectTemp() const { - auto value = byteSwap(i2c.readReg16(Regs::OutTemp)); + auto value = byteSwap(m_RegisterInterface.readReg16(Regs::OutTemp)); float result = (static_cast(value) / 340.0f) + 36.53f; return result; } template void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - const auto status = i2c.readReg(Regs::IntStatus); + 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 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(byteCount), readBuffer.size()) / sizeof(FifoSample) * sizeof(FifoSample); @@ -200,7 +200,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(&readBuffer[i]); diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 780aba8d..b495fe63 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -37,36 +37,36 @@ namespace SlimeVR::Sensors { template < - template + template typename T, - typename I2CImpl, + typename RegInterface, template typename Calibrator> class SoftFusionSensor : public Sensor { - using imu = T; + 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::type; using RawVectorT = std::array; 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; + using Calib = Calibrator; static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration; @@ -76,12 +76,12 @@ 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; @@ -139,9 +139,9 @@ class SoftFusionSensor : public Sensor { void processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) { if constexpr (!DirectTempReadOnly) { - const float scaledTemperature = imu::TemperatureBias + const float scaledTemperature = SensorType::TemperatureBias + static_cast(rawTemperature) - * (1.0 / imu::TemperatureSensitivity); + * (1.0 / SensorType::TemperatureSensitivity); lastReadTemperature = scaledTemperature; tempGradientCalculator.feedSample( @@ -202,20 +202,20 @@ class SoftFusionSensor : public Sensor { } public: - static constexpr auto TypeID = imu::Type; - static constexpr uint8_t Address = imu::Address; + static constexpr auto SensorTypeID = SensorType::Type; + static constexpr uint8_t Address = SensorType::Address; SoftFusionSensor( uint8_t id, - uint8_t i2cAddress, + RegInterface 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() { @@ -338,7 +338,7 @@ class SoftFusionSensor : public Sensor { bool initResult = false; if constexpr (Calib::HasMotionlessCalib) { - typename imu::MotionlessCalibrationData calibData; + typename SensorType::MotionlessCalibrationData calibData; std::memcpy( &calibData, calibrator.getMotionlessCalibrationData(), @@ -396,7 +396,7 @@ class SoftFusionSensor : public Sensor { SensorStatus getSensorState() final { return m_status; } SensorFusionRestDetect m_fusion; - imu m_sensor; + SensorType m_sensor; Calib calibrator{ m_fusion, m_sensor,