From 079919641ae910b460e791bcd0f08c5b1444b322 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Wed, 5 Feb 2025 18:11:18 +0100 Subject: [PATCH 01/37] Make SPI work Move sensor building logic to a separate class and file Don't use templates for RegisterInterface/I2CImpl/SPIImpl This started getting ridiculous, now we can actually maintain it. Make BNO085 work again Add BNO to automatic detection, remove a bunch of others Not all IMU types are enabled right now due to code size optimization, but it could be expanded in the future with optimization of Softfusion. Pick IMU type automatically by asking it --- src/consts.h | 1 + src/defines.h | 48 +- src/logging/Logger.cpp | 14 +- src/logging/Logger.h | 28 +- src/network/connection.h | 6 +- src/sensorinterface/RegisterInterface.h | 45 ++ src/sensorinterface/SPIImpl.h | 151 ++++++ .../softfusion => sensorinterface}/i2cimpl.h | 31 +- src/sensors/ADCResistanceSensor.cpp | 5 + src/sensors/ADCResistanceSensor.h | 6 +- src/sensors/EmptySensor.h | 2 +- src/sensors/ErroneousSensor.h | 2 +- src/sensors/SensorBuilder.h | 494 ++++++++++++++++++ src/sensors/SensorManager.cpp | 95 +--- src/sensors/SensorManager.h | 96 +--- src/sensors/bmi160sensor.h | 4 +- src/sensors/bno055sensor.h | 4 +- src/sensors/bno080sensor.h | 41 +- src/sensors/icm20948sensor.h | 4 +- src/sensors/mpu6050sensor.h | 4 +- src/sensors/mpu9250sensor.h | 4 +- src/sensors/sensor.h | 10 +- src/sensors/softfusion/drivers/bmi270.h | 123 +++-- src/sensors/softfusion/drivers/icm42688.h | 33 +- src/sensors/softfusion/drivers/icm45605.h | 11 +- src/sensors/softfusion/drivers/icm45686.h | 17 +- src/sensors/softfusion/drivers/icm45base.h | 48 +- .../softfusion/drivers/lsm6ds-common.h | 24 +- src/sensors/softfusion/drivers/lsm6ds3trc.h | 36 +- src/sensors/softfusion/drivers/lsm6dso.h | 29 +- src/sensors/softfusion/drivers/lsm6dsr.h | 44 +- src/sensors/softfusion/drivers/lsm6dsv.h | 50 +- src/sensors/softfusion/drivers/mpu6050.h | 55 +- src/sensors/softfusion/softfusionsensor.h | 83 ++- 34 files changed, 1154 insertions(+), 494 deletions(-) create mode 100644 src/sensorinterface/RegisterInterface.h create mode 100644 src/sensorinterface/SPIImpl.h rename src/{sensors/softfusion => sensorinterface}/i2cimpl.h (74%) create mode 100644 src/sensors/SensorBuilder.h diff --git a/src/consts.h b/src/consts.h index 74698f620..9c4f0f4c8 100644 --- a/src/consts.h +++ b/src/consts.h @@ -49,6 +49,7 @@ enum class SensorTypeID : uint8_t { Empty = 255 }; +#define IMU_AUTO SensorAuto #define IMU_UNKNOWN ErroneousSensor #define IMU_MPU9250 MPU9250Sensor #define IMU_MPU6500 MPU6050Sensor diff --git a/src/defines.h b/src/defines.h index f1c5ecc15..62cdf6f41 100644 --- a/src/defines.h +++ b/src/defines.h @@ -26,8 +26,8 @@ // ================================================ // Set parameters of IMU and board used -#define IMU IMU_BNO085 -#define SECOND_IMU IMU +#define IMU IMU_AUTO +#define SECOND_IMU IMU_AUTO #define BOARD BOARD_SLIMEVR #define IMU_ROTATION DEG_270 #define SECOND_IMU_ROTATION DEG_270 @@ -54,24 +54,24 @@ 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( \ + 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 +239,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 be8d68b9b..4508369be 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 09d1d2b9f..14e69d802 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/network/connection.h b/src/network/connection.h index 314d65da0..dc7d26c1d 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -137,9 +137,9 @@ class Connection { bool endBundle(); private: - void updateSensorState(std::vector>& sensors); + void updateSensorState(std::vector>& sensors); void maybeRequestFeatureFlags(); - bool isSensorStateUpdated(int i, std::unique_ptr& sensor); + bool isSensorStateUpdated(int i, std::unique_ptr<::Sensor>& sensor); bool beginPacket(); bool endPacket(); @@ -209,7 +209,7 @@ class Connection { void sendTrackerDiscovery(); // PACKET_SENSOR_INFO 15 - void sendSensorInfo(Sensor& sensor); + void sendSensorInfo(::Sensor& sensor); void sendAcknowledgeConfigChange(uint8_t sensorId, SensorToggles configType); diff --git a/src/sensorinterface/RegisterInterface.h b/src/sensorinterface/RegisterInterface.h new file mode 100644 index 000000000..91ad68627 --- /dev/null +++ b/src/sensorinterface/RegisterInterface.h @@ -0,0 +1,45 @@ +/* + 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 { + +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; + virtual std::string toString() const = 0; +}; + +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensorinterface/SPIImpl.h b/src/sensorinterface/SPIImpl.h new file mode 100644 index 000000000..9e4f046e8 --- /dev/null +++ b/src/sensorinterface/SPIImpl.h @@ -0,0 +1,151 @@ +/* + 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 { + +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 { return 0; } + + std::string toString() 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 diff --git a/src/sensors/softfusion/i2cimpl.h b/src/sensorinterface/i2cimpl.h similarity index 74% rename from src/sensors/softfusion/i2cimpl.h rename to src/sensorinterface/i2cimpl.h index 80ac421de..5dda198e7 100644 --- a/src/sensors/softfusion/i2cimpl.h +++ b/src/sensorinterface/i2cimpl.h @@ -23,25 +23,26 @@ #pragma once +#include + #include #include "I2Cdev.h" +#include "RegisterInterface.h" -namespace SlimeVR::Sensors::SoftFusion { - -struct I2CImpl { - static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2; +namespace SlimeVR::Sensors { +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 +53,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,16 +66,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; } + + std::string toString() const { + return std::string("I2C(") + std::to_string(m_devAddr) + std::string(")"); + } + private: uint8_t m_devAddr; }; -} // namespace SlimeVR::Sensors::SoftFusion +} // namespace SlimeVR::Sensors diff --git a/src/sensors/ADCResistanceSensor.cpp b/src/sensors/ADCResistanceSensor.cpp index 13cc8168e..cd92b0920 100644 --- a/src/sensors/ADCResistanceSensor.cpp +++ b/src/sensors/ADCResistanceSensor.cpp @@ -24,6 +24,8 @@ #include "GlobalVars.h" +namespace SlimeVR { +namespace Sensors { void ADCResistanceSensor::motionLoop() { #if ESP8266 float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution; @@ -39,3 +41,6 @@ void ADCResistanceSensor::motionLoop() { void ADCResistanceSensor::sendData() { networkConnection.sendFlexData(sensorId, m_Data); } + +} // namespace Sensors +} // namespace SlimeVR \ No newline at end of file diff --git a/src/sensors/ADCResistanceSensor.h b/src/sensors/ADCResistanceSensor.h index cd4a40f65..f0122324e 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -25,6 +25,8 @@ #include "sensor.h" #include "sensorinterface/SensorInterface.h" +namespace SlimeVR { +namespace Sensors { class ADCResistanceSensor : Sensor { public: static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE; @@ -40,7 +42,7 @@ class ADCResistanceSensor : Sensor { "ADCResistanceSensor", SensorTypeID::ADC_RESISTANCE, id, - pin, + *(new I2CImpl(0)), 0.0f, new SlimeVR::EmptySensorInterface ) @@ -67,3 +69,5 @@ class ADCResistanceSensor : Sensor { float m_Data = 0.0f; }; +} // namespace Sensors +} // namespace SlimeVR diff --git a/src/sensors/EmptySensor.h b/src/sensors/EmptySensor.h index 7f045a60a..65307f2b2 100644 --- a/src/sensors/EmptySensor.h +++ b/src/sensors/EmptySensor.h @@ -31,7 +31,7 @@ namespace Sensors { class EmptySensor : public Sensor { public: EmptySensor(uint8_t id) - : Sensor("EmptySensor", SensorTypeID::Empty, id, 0, 0.0){}; + : Sensor("EmptySensor", SensorTypeID::Empty, id, *(new I2CImpl(0)), 0.0){}; ~EmptySensor(){}; void motionSetup() override final{}; diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h index 7e3893085..0f94e2306 100644 --- a/src/sensors/ErroneousSensor.h +++ b/src/sensors/ErroneousSensor.h @@ -31,7 +31,7 @@ namespace Sensors { class ErroneousSensor : public Sensor { public: ErroneousSensor(uint8_t id, SensorTypeID type) - : Sensor("ErroneousSensor", type, id, 0, 0.0) + : Sensor("ErroneousSensor", type, id, *(new I2CImpl(0)), 0.0) , m_ExpectedType(type){}; ~ErroneousSensor(){}; diff --git a/src/sensors/SensorBuilder.h b/src/sensors/SensorBuilder.h new file mode 100644 index 000000000..1a391bb8c --- /dev/null +++ b/src/sensors/SensorBuilder.h @@ -0,0 +1,494 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + 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 + +#include "EmptySensor.h" +#include "ErroneousSensor.h" +#include "bmi160sensor.h" +#include "bno055sensor.h" +#include "bno080sensor.h" +#include "globals.h" +#include "icm20948sensor.h" +#include "logging/Logger.h" +#include "mpu6050sensor.h" +#include "mpu9250sensor.h" +#include "sensor.h" +#include "sensorinterface/DirectPinInterface.h" +#include "sensorinterface/I2CPCAInterface.h" +#include "sensorinterface/I2CWireSensorInterface.h" +#include "sensorinterface/MCP23X17PinInterface.h" +#include "sensorinterface/RegisterInterface.h" +#include "sensorinterface/SPIImpl.h" +#include "sensorinterface/SensorInterfaceManager.h" +#include "sensorinterface/i2cimpl.h" +#include "softfusion/drivers/bmi270.h" +#include "softfusion/drivers/icm42688.h" +#include "softfusion/drivers/icm45605.h" +#include "softfusion/drivers/icm45686.h" +#include "softfusion/drivers/lsm6ds3trc.h" +#include "softfusion/drivers/lsm6dso.h" +#include "softfusion/drivers/lsm6dsr.h" +#include "softfusion/drivers/lsm6dsv.h" +#include "softfusion/drivers/mpu6050.h" +#include "softfusion/softfusionsensor.h" + +#ifndef PRIMARY_IMU_ADDRESS_ONE +#define PRIMARY_IMU_ADDRESS_ONE 0 +#endif + +#ifndef SECONDARY_IMU_ADDRESS_TWO +#define SECONDARY_IMU_ADDRESS_TWO 0 +#endif + +#if USE_RUNTIME_CALIBRATION +#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h" +#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator +#else +#include "sensors/softfusion/SoftfusionCalibration.h" +#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator +#endif + +#if ESP32 +#include "driver/i2c.h" +#endif + +namespace SlimeVR { +namespace Sensors { +using SoftFusionLSM6DS3TRC + = SoftFusionSensor; +using SoftFusionICM42688 + = SoftFusionSensor; +using SoftFusionBMI270 = SoftFusionSensor; +using SoftFusionLSM6DSV = SoftFusionSensor; +using SoftFusionLSM6DSO = SoftFusionSensor; +using SoftFusionLSM6DSR = SoftFusionSensor; +using SoftFusionMPU6050 = SoftFusionSensor; +using SoftFusionICM45686 + = SoftFusionSensor; +using SoftFusionICM45605 + = SoftFusionSensor; +class SensorAuto {}; + +struct SensorBuilder { +public: + SensorManager* m_Manager; + SensorBuilder(SensorManager* sensorManager) + : m_Manager(sensorManager) {} + + uint8_t buildAllSensors() { + SensorInterfaceManager interfaceManager; + + uint8_t sensorID = 0; + uint8_t activeSensorCount = 0; + +#define NO_PIN nullptr +#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin) +#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) +#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) +#define PCA_WIRE(scl, sda, addr, ch) \ + interfaceManager.pcaWireInterface().get(scl, sda, addr, ch); +#define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ + *(new SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN)) + +#define SENSOR_DESC_ENTRY(ImuType, ...) \ + { \ + do { \ + std::unique_ptr<::Sensor> sensor; \ + if constexpr (std::is_same::value) { \ + auto sensorType = findSensorType(sensorID, __VA_ARGS__); \ + if (sensorType == SensorTypeID::Unknown) { \ + m_Manager->m_Logger.error( \ + "Can't find sensor type for sensor %d", \ + sensorID \ + ); \ + break; \ + } else { \ + m_Manager->m_Logger.info( \ + "Sensor %d automatically detected with %s", \ + sensorID, \ + getIMUNameByType(sensorType) \ + ); \ + sensor \ + = buildSensorDynamically(sensorType, sensorID, __VA_ARGS__); \ + } \ + } else { \ + sensor = buildSensor(sensorID, __VA_ARGS__); \ + } \ + if (sensor->isWorking()) { \ + m_Manager->m_Logger.info("Sensor %d configured", sensorID); \ + activeSensorCount++; \ + } \ + m_Manager->m_Sensors.push_back(std::move(sensor)); \ + } while (false); \ + sensorID++; \ + } + + // Apply descriptor list and expand to entries + SENSOR_DESC_LIST; + +#define SENSOR_INFO_ENTRY(ImuID, ...) \ + { m_Manager->m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); } + + // Apply sensor info list + SENSOR_INFO_LIST; + + return activeSensorCount; + } + +#define BUILD_SENSOR_ARGS \ + sensorID, imuInterface, rotation, sensorInterface, optional, intPin, extraParam + + std::unique_ptr<::Sensor> buildSensorDynamically( + SensorTypeID type, + uint8_t sensorID, + RegisterInterface& imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam + ) { + switch (type) { + // case SensorTypeID::MPU9250: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO080: + // return buildSensor(BUILD_SENSOR_ARGS); + case SensorTypeID::BNO085: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO055: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::MPU6050: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::BNO086: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BMI160: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM20948: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM42688: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::BMI270: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DS3TRC: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::LSM6DSV: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DSO: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::LSM6DSR: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::ICM45686: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM45605: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + default: + m_Manager->m_Logger.error( + "Unable to create sensor with type %s (%d)", + getIMUNameByType(type), + type + ); + } + return std::make_unique(sensorID); + } + + std::unique_ptr<::Sensor> buildSensorDynamically( + SensorTypeID type, + uint8_t sensorID, + uint8_t imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam + ) { + switch (type) { + // case SensorTypeID::MPU9250: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO080: + // return buildSensor(BUILD_SENSOR_ARGS); + case SensorTypeID::BNO085: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO055: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::MPU6050: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::BNO086: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BMI160: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM20948: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM42688: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::BMI270: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DS3TRC: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::LSM6DSV: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DSO: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::LSM6DSR: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::ICM45686: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM45605: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + default: + m_Manager->m_Logger.error( + "Unable to create sensor with type %s (%d)", + getIMUNameByType(type), + type + ); + } + return std::make_unique(sensorID); + } + + SensorTypeID findSensorType( + uint8_t sensorID, + uint8_t imuAddress, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam + ) { + sensorInterface->init(); + sensorInterface->swapIn(); + // if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuAddress)) + // { return SensorTypeID::LSM6DS3TRC; + // } + // if (SoftFusionICM42688::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::ICM42688; + //} + if (SoftFusionBMI270::checkPresent(sensorID, imuAddress)) { + return SensorTypeID::BMI270; + } + if (SoftFusionLSM6DSV::checkPresent(sensorID, imuAddress)) { + return SensorTypeID::LSM6DSV; + } + // if (SoftFusionLSM6DSO::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::LSM6DSO; + // } + // if (SoftFusionLSM6DSR::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::LSM6DSR; + // } + // if (SoftFusionMPU6050::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::MPU6050; + // } + if (SoftFusionICM45686::checkPresent(sensorID, imuAddress)) { + return SensorTypeID::ICM45686; + } + return BNO080Sensor::checkIfPresent(sensorID, imuAddress, intPin); + // if (SoftFusionICM45605::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::ICM45605; + // } + + return SensorTypeID::Unknown; + } + + SensorTypeID findSensorType( + uint8_t sensorID, + RegisterInterface& imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam + ) { + sensorInterface->init(); + sensorInterface->swapIn(); + // if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuInterface)) + // { return SensorTypeID::LSM6DS3TRC; + // } + // if (SoftFusionICM42688::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::ICM42688; + //} + if (SoftFusionBMI270::checkPresent(sensorID, imuInterface)) { + return SensorTypeID::BMI270; + } + if (SoftFusionLSM6DSV::checkPresent(sensorID, imuInterface)) { + return SensorTypeID::LSM6DSV; + } + // if (SoftFusionLSM6DSO::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::LSM6DSO; + // } + // if (SoftFusionLSM6DSR::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::LSM6DSR; + // } + // if (SoftFusionMPU6050::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::MPU6050; + // } + if (SoftFusionICM45686::checkPresent(sensorID, imuInterface)) { + return SensorTypeID::ICM45686; + } + return BNO080Sensor::checkIfPresent(sensorID, sensorInterface, intPin); + // if (SoftFusionICM45605::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::ICM45605; + // } + + return SensorTypeID::Unknown; + } + + template + std::unique_ptr<::Sensor> buildSensor( + uint8_t sensorID, + RegisterInterface& imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional = false, + PinInterface* intPin = nullptr, + int extraParam = 0 + ) { + return buildSensorReal( + sensorID, + imuInterface, + rotation, + sensorInterface, + optional, + intPin, + extraParam + ); + } + + template + std::unique_ptr<::Sensor> buildSensor( + uint8_t sensorID, + uint8_t imuAddress, + float rotation, + SensorInterface* sensorInterface, + bool optional = false, + PinInterface* intPin = nullptr, + int extraParam = 0 + ) { + uint8_t address = imuAddress > 0 ? imuAddress : ImuType::Address + sensorID; + return buildSensorReal( + sensorID, + *(new I2CImpl(address)), + rotation, + sensorInterface, + optional, + intPin, + extraParam + ); + } + + template + std::unique_ptr<::Sensor> buildSensorReal( + uint8_t sensorID, + RegisterInterface& imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional = false, + PinInterface* intPin = nullptr, + int extraParam = 0 + ) { + m_Manager->m_Logger.trace( + "Building IMU with: id=%d,\n\ + address=%s, rotation=%f,\n\ + interface=%s, int=%s, extraParam=%d, optional=%d", + sensorID, + imuInterface.toString(), + rotation, + sensorInterface, + intPin, + extraParam, + optional + ); + + // Now start detecting and building the IMU + std::unique_ptr<::Sensor> sensor; + + // Init I2C bus for each sensor upon startup + sensorInterface->init(); + sensorInterface->swapIn(); + + if (imuInterface.hasSensorOnBus()) { + m_Manager->m_Logger.trace( + "Sensor %d found at address %s", + sensorID + 1, + imuInterface.toString() + ); + } else { + if (!optional) { + m_Manager->m_Logger.error( + "Mandatory sensor %d not found at address %s", + sensorID + 1, + imuInterface.toString() + ); + sensor = std::make_unique(sensorID, ImuType::TypeID); + } else { + m_Manager->m_Logger.debug( + "Optional sensor %d not found at address %s", + sensorID + 1, + imuInterface.toString() + ); + sensor = std::make_unique(sensorID); + } + return sensor; + } + + sensor = std::make_unique( + sensorID, + imuInterface, + rotation, + sensorInterface, + intPin, + extraParam + ); + + sensor->motionSetup(); + return sensor; + } +}; +} // namespace Sensors +} // namespace SlimeVR diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 505b17afc..702321af2 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -23,107 +23,19 @@ #include "SensorManager.h" -#include -#include - -#include "bmi160sensor.h" -#include "bno055sensor.h" -#include "bno080sensor.h" -#include "icm20948sensor.h" -#include "mpu6050sensor.h" -#include "mpu9250sensor.h" -#include "sensorinterface/SensorInterfaceManager.h" -#include "sensors/softfusion/SoftfusionCalibration.h" -#include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h" -#include "softfusion/drivers/bmi270.h" -#include "softfusion/drivers/icm42688.h" -#include "softfusion/drivers/icm45605.h" -#include "softfusion/drivers/icm45686.h" -#include "softfusion/drivers/lsm6ds3trc.h" -#include "softfusion/drivers/lsm6dso.h" -#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 -#include "driver/i2c.h" -#endif - -#if USE_RUNTIME_CALIBRATION -#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator -#else -#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator -#endif +#include "SensorBuilder.h" namespace SlimeVR { namespace Sensors { -using SoftFusionLSM6DS3TRC = SoftFusionSensor< - SoftFusion::Drivers::LSM6DS3TRC, - SoftFusion::I2CImpl, - SFCALIBRATOR>; -using SoftFusionICM42688 = SoftFusionSensor< - SoftFusion::Drivers::ICM42688, - SoftFusion::I2CImpl, - SFCALIBRATOR>; -using SoftFusionBMI270 - = SoftFusionSensor; -using SoftFusionLSM6DSV - = SoftFusionSensor; -using SoftFusionLSM6DSO - = SoftFusionSensor; -using SoftFusionLSM6DSR - = SoftFusionSensor; -using SoftFusionMPU6050 - = SoftFusionSensor; -using SoftFusionICM45686 = SoftFusionSensor< - SoftFusion::Drivers::ICM45686, - SoftFusion::I2CImpl, - SFCALIBRATOR>; -using SoftFusionICM45605 = SoftFusionSensor< - SoftFusion::Drivers::ICM45605, - SoftFusion::I2CImpl, - SFCALIBRATOR>; void SensorManager::setup() { - SensorInterfaceManager interfaceManager; - - uint8_t sensorID = 0; - uint8_t activeSensorCount = 0; if (m_MCP.begin_I2C()) { m_Logger.info("MCP initialized"); } -#define NO_PIN nullptr -#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin) -#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) -#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) -#define PCA_WIRE(scl, sda, addr, ch) \ - interfaceManager.pcaWireInterface().get(scl, sda, addr, ch); - -#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++; \ - } - - // Apply descriptor list and expand to entries - SENSOR_DESC_LIST; + SensorBuilder sensorBuilder = SensorBuilder(this); + uint8_t activeSensorCount = sensorBuilder.buildAllSensors(); -#define SENSOR_INFO_ENTRY(ImuID, ...) \ - { 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) { @@ -209,5 +121,6 @@ void SensorManager::update() { networkConnection.endBundle(); #endif } + } // namespace Sensors } // namespace SlimeVR diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 8be7a097b..bca457e86 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -31,32 +31,16 @@ #include "ErroneousSensor.h" #include "globals.h" #include "logging/Logger.h" -#include "sensor.h" -#include "sensoraddress.h" #include "sensorinterface/DirectPinInterface.h" #include "sensorinterface/I2CPCAInterface.h" #include "sensorinterface/I2CWireSensorInterface.h" #include "sensorinterface/MCP23X17PinInterface.h" +#include "sensorinterface/RegisterInterface.h" +#include "sensorinterface/i2cimpl.h" namespace SlimeVR { namespace Sensors { -#ifndef PRIMARY_IMU_ADDRESS_ONE -#define PRIMARY_IMU_ADDRESS_ONE true -#endif - -#ifndef PRIMARY_IMU_ADDRESS_TWO -#define PRIMARY_IMU_ADDRESS_TWO false -#endif - -#ifndef SECONDARY_IMU_ADDRESS_TWO -#define SECONDARY_IMU_ADDRESS_TWO false -#endif - -#ifndef SECONDARY_IMU_ADDRESS_ONE -#define SECONDARY_IMU_ADDRESS_ONE true -#endif - class SensorManager { public: SensorManager() @@ -80,81 +64,9 @@ class SensorManager { std::vector> m_Sensors; Adafruit_MCP23X17 m_MCP; - template - std::unique_ptr<::Sensor> buildSensor( - uint8_t sensorID, - ImuAddress imuAddress, - float rotation, - SensorInterface* sensorInterface, - bool optional = false, - PinInterface* intPin = nullptr, - int extraParam = 0 - ) { - uint8_t i2cAddress = imuAddress.getAddress(ImuType::Address); - 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, - rotation, - sensorInterface, - intPin, - extraParam, - optional - ); - - // Now start detecting and building the IMU - std::unique_ptr<::Sensor> sensor; - - // Init I2C bus for each sensor upon startup - sensorInterface->init(); - sensorInterface->swapIn(); - - if (I2CSCAN::hasDevOnBus(i2cAddress)) { - m_Logger - .trace("Sensor %d found at address 0x%02X", sensorID + 1, i2cAddress); - } else if (I2CSCAN::hasDevOnBus(i2cAddress - )) { // scan again because ICM45* may not respond on first I2C - // transaction - m_Logger.trace( - "Sensor %d found at address 0x%02X on second scan", - sensorID + 1, - i2cAddress - ); - } else { - if (!optional) { - m_Logger.error( - "Mandatory sensor %d not found at address 0x%02X", - sensorID + 1, - i2cAddress - ); - sensor = std::make_unique(sensorID, ImuType::TypeID); - } else { - m_Logger.debug( - "Optional sensor %d not found at address 0x%02X", - sensorID + 1, - i2cAddress - ); - sensor = std::make_unique(sensorID); - } - return sensor; - } - - sensor = std::make_unique( - sensorID, - i2cAddress, - rotation, - sensorInterface, - intPin, - extraParam - ); - - sensor->motionSetup(); - return sensor; - } - uint32_t m_LastBundleSentAtMicros = micros(); + + friend class SensorBuilder; }; } // namespace Sensors } // namespace SlimeVR diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index b0aa0f417..bc49d2dd1 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -139,7 +139,7 @@ class BMI160Sensor : public Sensor { BMI160Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface*, @@ -149,7 +149,7 @@ class BMI160Sensor : public Sensor { "BMI160Sensor", SensorTypeID::BMI160, id, - i2cAddress, + registerInterface, rotation, sensorInterface ) diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 16fb6b020..bc5fb89b5 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -35,7 +35,7 @@ class BNO055Sensor : public Sensor { BNO055Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface*, @@ -45,7 +45,7 @@ class BNO055Sensor : public Sensor { "BNO055Sensor", SensorTypeID::BNO055, id, - i2cAddress, + registerInterface, rotation, sensorInterface ){}; diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index df1c6b983..9d4e2804a 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -25,6 +25,7 @@ #define SENSORS_BNO080SENSOR_H #include +#include #include "sensor.h" @@ -37,7 +38,7 @@ class BNO080Sensor : public Sensor { BNO080Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface* intPin, @@ -47,7 +48,7 @@ class BNO080Sensor : public Sensor { "BNO080Sensor", SensorTypeID::BNO080, id, - i2cAddress, + registerInterface, rotation, sensorInterface ) @@ -63,19 +64,43 @@ class BNO080Sensor : public Sensor { bool isFlagSupported(SensorToggles toggle) const final; void sendTempIfNeeded(); + static SensorTypeID checkIfPresent( + uint8_t sensorID, + SlimeVR::SensorInterface* sensorInterface, + PinInterface* intPin + ) { + // Lazy check for if BNO is present, we only check if I2C has an address here + if (I2CSCAN::hasDevOnBus(Address + sensorID)) { + return SensorTypeID::BNO085; // Assume it's 085, more precise diff will + // require talking to it + } + return SensorTypeID::Unknown; + } + + static SensorTypeID + checkIfPresent(uint8_t sensorID, uint8_t imuAddress, PinInterface* intPin) { + uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID; + // Lazy check for if BNO is present, we only check if I2C has an address here + if (I2CSCAN::hasDevOnBus(address)) { + return SensorTypeID::BNO085; // Assume it's 085, more precise diff will + // require talking to it + } + return SensorTypeID::Unknown; + } + protected: // forwarding constructor BNO080Sensor( const char* sensorName, SensorTypeID imuId, uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface* intPin, int ) - : Sensor(sensorName, imuId, id, i2cAddress, rotation, sensorInterface) + : Sensor(sensorName, imuId, id, registerInterface, rotation, sensorInterface) , m_IntPin(intPin){}; private: @@ -107,7 +132,7 @@ class BNO085Sensor : public BNO080Sensor { static constexpr auto TypeID = SensorTypeID::BNO085; BNO085Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface* intPin, @@ -117,7 +142,7 @@ class BNO085Sensor : public BNO080Sensor { "BNO085Sensor", SensorTypeID::BNO085, id, - i2cAddress, + registerInterface, rotation, sensorInterface, intPin, @@ -130,7 +155,7 @@ class BNO086Sensor : public BNO080Sensor { static constexpr auto TypeID = SensorTypeID::BNO086; BNO086Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface* intPin, @@ -140,7 +165,7 @@ class BNO086Sensor : public BNO080Sensor { "BNO086Sensor", SensorTypeID::BNO086, id, - i2cAddress, + registerInterface, rotation, sensorInterface, intPin, diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 469b67160..53ece5fbb 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -35,7 +35,7 @@ class ICM20948Sensor : public Sensor { ICM20948Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface*, @@ -45,7 +45,7 @@ class ICM20948Sensor : public Sensor { "ICM20948Sensor", SensorTypeID::ICM20948, id, - i2cAddress, + registerInterface, rotation, sensorInterface ) {} diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index d81f43c6c..67efeecfa 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -36,7 +36,7 @@ class MPU6050Sensor : public Sensor { MPU6050Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface*, @@ -46,7 +46,7 @@ class MPU6050Sensor : public Sensor { "MPU6050Sensor", SensorTypeID::MPU6050, id, - i2cAddress, + registerInterface, rotation, sensorInterface ){}; diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index b8e9d5724..6e005da2d 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -49,7 +49,7 @@ class MPU9250Sensor : public Sensor { MPU9250Sensor( uint8_t id, - uint8_t i2cAddress, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface, PinInterface*, @@ -59,7 +59,7 @@ class MPU9250Sensor : public Sensor { "MPU9250Sensor", SensorTypeID::MPU9250, id, - i2cAddress, + registerInterface, rotation, sensorInterface ) diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 81c8e7912..72102d2ef 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -35,7 +35,9 @@ #include "configuration/Configuration.h" #include "globals.h" #include "logging/Logger.h" +#include "sensorinterface/RegisterInterface.h" #include "sensorinterface/SensorInterface.h" +#include "sensorinterface/i2cimpl.h" #include "status/TPSCounter.h" #include "utils.h" @@ -54,12 +56,12 @@ class Sensor { const char* sensorName, SensorTypeID type, uint8_t id, - uint8_t address, + SlimeVR::Sensors::RegisterInterface& registerInterface, float rotation, SlimeVR::SensorInterface* sensorInterface = nullptr ) : m_hwInterface(sensorInterface) - , addr(address) + , m_RegisterInterface(registerInterface) , sensorId(id) , sensorType(type) , sensorOffset({Quat(Vector3(0, 0, 1), rotation)}) @@ -67,6 +69,7 @@ class Sensor { char buf[4]; sprintf(buf, "%u", id); m_Logger.setTag(buf); + addr = registerInterface.getAddress(); } virtual ~Sensor(){}; @@ -112,7 +115,8 @@ class Sensor { SlimeVR::SensorInterface* m_hwInterface = nullptr; protected: - uint8_t addr = 0; + SlimeVR::Sensors::RegisterInterface& m_RegisterInterface; + uint8_t addr; uint8_t sensorId = 0; SensorTypeID sensorType = SensorTypeID::Unknown; bool working = false; diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index ab139c054..be2b7805b 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -29,6 +29,7 @@ #include #include +#include "../../../sensorinterface/RegisterInterface.h" #include "bmi270fw.h" #include "vqf.h" @@ -39,7 +40,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 400Hz, accel ODR = 100Hz // Timestamps reading are not used -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) {} + RegisterInterface& m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + int8_t m_zxFactor; + BMI270(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) + , m_zxFactor(0) {} struct Regs { struct WhoAmI { @@ -244,14 +244,20 @@ 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 +267,63 @@ 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 + static_cast(sizeof(bmi270_firmware) - pos), + 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 +342,54 @@ 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 +418,12 @@ 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 +440,14 @@ 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 +476,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 767607255..a228ab60d 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -36,7 +36,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 500Hz, accel ODR = 100Hz // Timestamps reading not used, as they're useless (constant predefined increment) -template struct ICM42688 { static constexpr uint8_t Address = 0x68; static constexpr auto Name = "ICM-42688"; @@ -66,11 +65,11 @@ struct ICM42688 { .restThAcc = 0.196f, }; - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + RegisterInterface& m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + ICM42688(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} struct Regs { struct WhoAmI { @@ -143,15 +142,18 @@ 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 +165,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 +173,8 @@ 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 fbc54686e..2b7f0d064 100644 --- a/src/sensors/softfusion/drivers/icm45605.h +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -35,8 +35,7 @@ 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 { +struct ICM45605 : public ICM45Base { static constexpr auto Name = "ICM-45605"; static constexpr auto Type = SensorTypeID::ICM45605; @@ -48,8 +47,8 @@ struct ICM45605 : public ICM45Base { .restThAcc = 0.0098f, }; - ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : ICM45Base{i2c, logger} {} + ICM45605(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger) + : ICM45Base{registerInterface, logger} {} struct Regs { struct WhoAmI { @@ -59,8 +58,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 1306b8f94..9b93500ca 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -35,8 +35,7 @@ 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 { +struct ICM45686 : public ICM45Base { static constexpr auto Name = "ICM-45686"; static constexpr auto Type = SensorTypeID::ICM45686; @@ -48,8 +47,8 @@ struct ICM45686 : public ICM45Base { .restThAcc = 0.196f, }; - ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : ICM45Base{i2c, logger} {} + ICM45686(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger) + : ICM45Base{registerInterface, logger} {} struct Regs { struct WhoAmI { @@ -68,13 +67,11 @@ struct ICM45686 : public ICM45Base { }; }; - using ICM45Base::i2c; - 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 907daf88c..57e6dede2 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -24,6 +24,8 @@ #include #include +#include "../../../sensorinterface/RegisterInterface.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 32g @@ -33,7 +35,6 @@ 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 struct ICM45Base { static constexpr uint8_t Address = 0x68; @@ -53,11 +54,11 @@ struct ICM45Base { static constexpr bool Uses32BitSensorData = true; - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + RegisterInterface& m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + ICM45Base(RegisterInterface& registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} struct BaseRegs { static constexpr uint8_t TempData = 0x0c; @@ -119,17 +120,35 @@ struct ICM45Base { static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; 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; @@ -148,7 +167,7 @@ struct ICM45Base { constexpr int16_t InvalidReading = -32768; - size_t fifo_packets = i2c.readReg16(BaseRegs::FifoCount); + size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount); if (fifo_packets >= 1) { // @@ -178,7 +197,8 @@ struct ICM45Base { fifo_packets = std::min(fifo_packets, MaxReadings); size_t bytes_to_read = fifo_packets * FullFifoEntrySize; - i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data()); + m_RegisterInterface + .readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data()); for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { uint8_t header = read_buffer[i]; diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 24c617734..12c2fcfa2 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -27,16 +27,20 @@ #include #include +#include "../../../sensorinterface/RegisterInterface.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { -template struct LSM6DSOutputHandler { - LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + LSM6DSOutputHandler( + RegisterInterface& registerInterface, + SlimeVR::Logging::Logger& logger + ) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; + RegisterInterface& m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; #pragma pack(push, 1) struct FifoEntryAligned { @@ -61,12 +65,13 @@ 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 +81,8 @@ 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 e2e44e6a5..48a654d2b 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -27,6 +27,7 @@ #include #include +#include "../../../sensorinterface/RegisterInterface.h" #include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -35,7 +36,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 416Hz -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) {} + RegisterInterface& m_RegisterInterface; + SlimeVR::Logging::Logger m_Logger; + LSM6DS3TRC(RegisterInterface& 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 17d17bc2d..56cef771c 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -36,8 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 104Hz -template -struct LSM6DSO : LSM6DSOutputHandler { +struct LSM6DSO : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSO"; static constexpr auto Type = SensorTypeID::LSM6DSO; @@ -68,8 +67,6 @@ struct LSM6DSO : LSM6DSOutputHandler { .restThAcc = 0.192f, }; - using LSM6DSOutputHandler::i2c; - struct Regs { struct WhoAmI { static constexpr uint8_t reg = 0x0f; @@ -104,18 +101,24 @@ struct LSM6DSO : LSM6DSOutputHandler { static constexpr uint8_t FifoData = 0x78; }; - LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSO(RegisterInterface& 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 +128,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 a9d7ac1da..e1ec127d4 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -35,8 +35,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 104Hz -template -struct LSM6DSR : LSM6DSOutputHandler { +struct LSM6DSR : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSR"; static constexpr auto Type = SensorTypeID::LSM6DSR; @@ -67,8 +66,6 @@ struct LSM6DSR : LSM6DSOutputHandler { .restThAcc = 0.192f, }; - using LSM6DSOutputHandler::i2c; - struct Regs { struct WhoAmI { static constexpr uint8_t reg = 0x0f; @@ -103,18 +100,24 @@ struct LSM6DSR : LSM6DSOutputHandler { static constexpr uint8_t FifoData = 0x78; }; - LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSR(RegisterInterface& 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,15 +127,14 @@ struct LSM6DSR : LSM6DSOutputHandler { GyroCall&& processGyroSample, TempCall&& processTempSample ) { - LSM6DSOutputHandler:: - template bulkRead( - processAccelSample, - processGyroSample, - processTempSample, - GyrTs, - AccTs, - TempTs - ); + LSM6DSOutputHandler::template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index 60c9a9bfc..c715d12ff 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -36,8 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 480Hz, accel ODR = 120Hz -template -struct LSM6DSV : LSM6DSOutputHandler { +struct LSM6DSV : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSV"; static constexpr auto Type = SensorTypeID::LSM6DSV; @@ -68,8 +67,6 @@ struct LSM6DSV : LSM6DSOutputHandler { .restThAcc = 0.192f, }; - using LSM6DSOutputHandler::i2c; - struct Regs { struct WhoAmI { static constexpr uint8_t reg = 0x0f; @@ -116,21 +113,27 @@ struct LSM6DSV : LSM6DSOutputHandler { static constexpr uint8_t FifoData = 0x78; }; - LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSV(RegisterInterface& 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,15 +143,14 @@ struct LSM6DSV : LSM6DSOutputHandler { GyroCall&& processGyroSample, TempCall&& processTempSample ) { - LSM6DSOutputHandler:: - template bulkRead( - processAccelSample, - processGyroSample, - processTempSample, - GyrTs, - AccTs, - TempTs - ); + LSM6DSOutputHandler::bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 79c24ba60..d422804a9 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -29,6 +29,7 @@ #include #include +#include "../../../sensorinterface/RegisterInterface.h" #include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -37,7 +38,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = accel ODR = 250Hz -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) {} + RegisterInterface& m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + MPU6050(RegisterInterface& i2c, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(i2c) + , m_Logger(logger) {} struct Regs { struct WhoAmI { @@ -120,49 +120,52 @@ 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 +176,30 @@ 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); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + 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 +207,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 2d808ae6d..c3455cb56 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -27,6 +27,7 @@ #include #include +#include "../../sensorinterface/i2cimpl.h" #include "../RestCalibrationDetector.h" #include "../SensorFusionRestDetect.h" #include "../sensor.h" @@ -37,36 +38,35 @@ namespace SlimeVR::Sensors { template < - template typename T, - typename I2CImpl, 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,13 @@ 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 +140,10 @@ class SoftFusionSensor : public Sensor { void processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) { if constexpr (!DirectTempReadOnly) { - const float scaledTemperature = imu::TemperatureBias - + static_cast(rawTemperature) - * (1.0 / imu::TemperatureSensitivity); + const float scaledTemperature + = SensorType::TemperatureBias + + static_cast(rawTemperature) + * (1.0 / SensorType::TemperatureSensitivity); lastReadTemperature = scaledTemperature; if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) { @@ -204,20 +206,32 @@ class SoftFusionSensor : public Sensor { } public: - static constexpr auto TypeID = imu::Type; - static constexpr uint8_t Address = imu::Address; + static constexpr auto TypeID = SensorType::Type; + static constexpr uint8_t Address = SensorType::Address; SoftFusionSensor( uint8_t id, - uint8_t i2cAddress, + RegisterInterface& 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() { @@ -346,7 +360,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(), @@ -409,7 +423,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, @@ -428,6 +442,25 @@ class SoftFusionSensor : public Sensor { uint32_t m_lastTemperaturePacketSent = 0; RestCalibrationDetector calibrationDetector; + + static bool checkPresent(uint8_t sensorID, const RegisterInterface& imuInterface) { + I2Cdev::readTimeout = 100; + auto value = imuInterface.readReg(SensorType::Regs::WhoAmI::reg); + I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; + if (SensorType::Regs::WhoAmI::value == value) { + return true; + } + return false; + } + + static bool checkPresent(uint8_t sensorID, uint8_t imuAddress) { + uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID; + if (!I2CSCAN::hasDevOnBus(address)) { + return false; + } + const I2CImpl& i2c = I2CImpl(address); + return checkPresent(sensorID, i2c); + } }; } // namespace SlimeVR::Sensors From f5df7534dbb933efe16ffb21c3961fdf78306afd Mon Sep 17 00:00:00 2001 From: Butterscotch! Date: Thu, 27 Feb 2025 17:47:18 -0500 Subject: [PATCH 02/37] ESP32 spelling fix (#396) Fix: ES32 -> ESP32 --- platformio-tools.ini | 4 ++-- src/consts.h | 4 ++-- src/defines.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/platformio-tools.ini b/platformio-tools.ini index 3098d22a4..f6c42399e 100644 --- a/platformio-tools.ini +++ b/platformio-tools.ini @@ -72,7 +72,7 @@ build_flags = -DESP32C3 board = dfrobot_beetle_esp32c3 -[env:BOARD_ES32C3DEVKITM1] +[env:BOARD_ESP32C3DEVKITM1] platform = espressif32 @ 6.7.0 platform_packages = framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 @@ -82,7 +82,7 @@ build_flags = -DESP32C3 board = esp32-c3-devkitm-1 -[env:BOARD_ES32C6DEVKITC1] +[env:BOARD_ESP32C6DEVKITC1] platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.06.11/platform-espressif32.zip build_flags = ${env.build_flags} diff --git a/src/consts.h b/src/consts.h index 9c4f0f4c8..784851f77 100644 --- a/src/consts.h +++ b/src/consts.h @@ -84,14 +84,14 @@ enum class SensorTypeID : uint8_t { #define BOARD_SLIMEVR 9 #define BOARD_LOLIN_C3_MINI 10 #define BOARD_BEETLE32C3 11 -#define BOARD_ES32C3DEVKITM1 12 +#define BOARD_ESP32C3DEVKITM1 12 #define BOARD_OWOTRACK 13 // Only used by owoTrack mobile app #define BOARD_WRANGLER 14 // Only used by wrangler app #define BOARD_MOCOPI 15 // Used by mocopi/moslime #define BOARD_WEMOSWROOM02 16 #define BOARD_XIAO_ESP32C3 17 #define BOARD_HARITORA 18 // Used by Haritora/SlimeTora -#define BOARD_ES32C6DEVKITC1 19 +#define BOARD_ESP32C6DEVKITC1 19 #define BOARD_GLOVE_IMU_SLIMEVR_DEV 20 // IMU Glove #define BOARD_GESTURES 21 // Used by Gestures #define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware diff --git a/src/defines.h b/src/defines.h index 62cdf6f41..42492f356 100644 --- a/src/defines.h +++ b/src/defines.h @@ -331,7 +331,7 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ #define PIN_BATTERY_LEVEL 3 #define LED_PIN 10 #define LED_INVERTED false -#elif BOARD == BOARD_ES32C3DEVKITM1 || BOARD == BOARD_ES32C6DEVKITC1 +#elif BOARD == BOARD_ESP32C3DEVKITM1 || BOARD == BOARD_ESP32C6DEVKITC1 #define PIN_IMU_SDA 5 #define PIN_IMU_SCL 4 #define PIN_IMU_INT 6 From 3d647f0c8b3060e80c3eeb42dadde24b2905174f Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Tue, 4 Mar 2025 18:54:57 +0100 Subject: [PATCH 03/37] (Probably) multiply acceleration by tracker rotation offset --- lib/math/quat.cpp | 9 +++++++++ lib/math/quat.h | 2 ++ src/main.cpp | 2 +- src/sensors/sensor.cpp | 1 + 4 files changed, 13 insertions(+), 1 deletion(-) diff --git a/lib/math/quat.cpp b/lib/math/quat.cpp index af78fa3b2..c2d7d4680 100644 --- a/lib/math/quat.cpp +++ b/lib/math/quat.cpp @@ -229,3 +229,12 @@ void Quat::set_axis_angle(const Vector3& axis, const float& angle) { cos_angle); } } + +void Quat::sandwitch(Vector3& v) { + float tempX, tempY; + tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x; + tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x * v.y; + v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x - y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z; + v.x = tempX; + v.y = tempY; +} diff --git a/lib/math/quat.h b/lib/math/quat.h index 2110f43d0..5383e311f 100644 --- a/lib/math/quat.h +++ b/lib/math/quat.h @@ -79,6 +79,8 @@ class Quat { Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq, const float& t) const; bool equalsWithEpsilon(const Quat& q2); + void sandwitch(Vector3& vector); + void set_axis_angle(const Vector3& axis, const float& angle); inline void get_axis_angle(Vector3& r_axis, double& r_angle) const { r_angle = 2 * std::acos(w); diff --git a/src/main.cpp b/src/main.cpp index 1dca40bfb..0b6746325 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -78,7 +78,7 @@ void setup() { // this, check needs to be re-added. auto clearResult = I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); if (clearResult != 0) { - logger.error("Can't clear I2C bus, error %d", clearResult); + logger.warn("Can't clear I2C bus, error %d", clearResult); } // join I2C bus diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 3e66e0c82..4e12b2dd9 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -33,6 +33,7 @@ SensorStatus Sensor::getSensorState() { void Sensor::setAcceleration(Vector3 a) { acceleration = a; + sensorOffset.sandwitch(acceleration); newAcceleration = true; } From 6f515bd2ba4d176fab1c8f0cfd06c8b16e557e58 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:08:36 +0200 Subject: [PATCH 04/37] Remove stome stuff from softfusionsensor --- src/debug.h | 2 +- src/defines.h | 2 +- src/sensors/SensorManager.cpp | 10 +- src/sensors/softfusion/CalibrationBase.h | 30 +-- .../softfusion/SoftfusionCalibration.h | 171 ++++++++++----- src/sensors/softfusion/drivers/bmi270.h | 12 +- src/sensors/softfusion/drivers/callbacks.h | 34 +++ src/sensors/softfusion/drivers/icm42688.h | 17 +- src/sensors/softfusion/drivers/icm45base.h | 15 +- .../softfusion/drivers/lsm6ds-common.h | 16 +- src/sensors/softfusion/drivers/lsm6ds3trc.h | 17 +- src/sensors/softfusion/drivers/lsm6dso.h | 23 +- src/sensors/softfusion/drivers/lsm6dsr.h | 22 +- src/sensors/softfusion/drivers/lsm6dsv.h | 22 +- src/sensors/softfusion/drivers/mpu6050.h | 8 +- src/sensors/softfusion/imuconsts.h | 56 +++++ .../runtimecalibration/RuntimeCalibration.h | 54 +++-- src/sensors/softfusion/softfusionsensor.h | 205 +++++------------- 18 files changed, 367 insertions(+), 349 deletions(-) create mode 100644 src/sensors/softfusion/drivers/callbacks.h create mode 100644 src/sensors/softfusion/imuconsts.h diff --git a/src/debug.h b/src/debug.h index cf49ea4ff..55c678498 100644 --- a/src/debug.h +++ b/src/debug.h @@ -101,7 +101,7 @@ #endif #ifndef USE_RUNTIME_CALIBRATION -#define USE_RUNTIME_CALIBRATION true +#define USE_RUNTIME_CALIBRATION false #endif #define DEBUG_MEASURE_SENSOR_TIME_TAKEN false diff --git a/src/defines.h b/src/defines.h index f1c5ecc15..833245cf6 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_LSM6DSV #define SECOND_IMU IMU #define BOARD BOARD_SLIMEVR #define IMU_ROTATION DEG_270 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 505b17afc..4a5fe5906 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -52,13 +52,12 @@ #endif #if USE_RUNTIME_CALIBRATION -#define SFCALIBRATOR SlimeVR::Sensors::RuntimeCalibration::RuntimeCalibrator +#define SFCALIBRATOR RuntimeCalibration::RuntimeCalibrator #else -#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator +#define SFCALIBRATOR SoftfusionCalibrator #endif -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { using SoftFusionLSM6DS3TRC = SoftFusionSensor< SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl, @@ -209,5 +208,4 @@ void SensorManager::update() { networkConnection.endBundle(); #endif } -} // namespace Sensors -} // namespace SlimeVR +} // namespace SlimeVR::Sensors diff --git a/src/sensors/softfusion/CalibrationBase.h b/src/sensors/softfusion/CalibrationBase.h index bef0d9c59..91ae20d8e 100644 --- a/src/sensors/softfusion/CalibrationBase.h +++ b/src/sensors/softfusion/CalibrationBase.h @@ -27,13 +27,14 @@ #include #include +#include "../SensorFusionRestDetect.h" #include "configuration/SensorConfig.h" +#include "imuconsts.h" #include "motionprocessing/types.h" -#include "sensors/SensorFusionRestDetect.h" -namespace SlimeVR::Sensor { +namespace SlimeVR::Sensors { -template +template class CalibrationBase { public: CalibrationBase( @@ -41,20 +42,18 @@ class CalibrationBase { 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; + using RawSensorT = typename Consts::RawSensorT; + using RawVectorT = typename Consts::RawVectorT; + static constexpr bool HasMotionlessCalib = requires(IMU& i) { typename IMU::MotionlessCalibrationData; }; static constexpr size_t MotionlessCalibDataSize() { @@ -69,11 +68,9 @@ class CalibrationBase { using ReturnLastFn = std::function(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 +113,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 diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h index b054020bb..6c47bf97f 100644 --- a/src/sensors/softfusion/SoftfusionCalibration.h +++ b/src/sensors/softfusion/SoftfusionCalibration.h @@ -23,48 +23,120 @@ #pragma once +#include #include #include +#include "../SensorFusionRestDetect.h" +#include "CalibrationBase.h" #include "GlobalVars.h" #include "configuration/SensorConfig.h" #include "logging/Logger.h" #include "motionprocessing/RestDetection.h" #include "motionprocessing/types.h" -#include "sensors/SensorFusionRestDetect.h" -#include "sensors/softfusion/CalibrationBase.h" -namespace SlimeVR::Sensor { +namespace SlimeVR::Sensors { -template -class SoftfusionCalibrator : public CalibrationBase { +template +class SoftfusionCalibrator : public CalibrationBase { public: static constexpr bool HasUpsideDownCalibration = true; - using Base = CalibrationBase; + using Base = CalibrationBase; + using Consts = typename Base::Consts; + using RawSensorT = typename Consts::RawSensorT; + using RawVectorT = typename Consts::RawVectorT; SoftfusionCalibrator( Sensors::SensorFusionRestDetect& fusion, IMU& sensor, uint8_t sensorId, SlimeVR::Logging::Logger& logger, - float TempTs, - float AScale, - float GScale, SensorToggleState& toggles ) - : Base{fusion, sensor, sensorId, logger, TempTs, AScale, GScale, toggles} { - calibration.T_Ts = TempTs; + : Base{fusion, sensor, sensorId, logger, toggles} { + calibration.T_Ts = IMU::TempTs; } - void startCalibration( - int calibrationType, - const Base::EatSamplesFn& eatSamplesForSeconds, - const Base::ReturnLastFn& eatSamplesReturnLast - ) final { + void eatSamplesForSeconds(const uint32_t seconds) { + const auto targetDelay = millis() + 1000 * seconds; + auto lastSecondsRemaining = seconds; + while (millis() < targetDelay) { +#ifdef ESP8266 + ESP.wdtFeed(); +#endif + auto currentSecondsRemaining = (targetDelay - millis()) / 1000; + if (currentSecondsRemaining != lastSecondsRemaining) { + logger.info("%d...", currentSecondsRemaining + 1); + lastSecondsRemaining = currentSecondsRemaining; + } + + sensor.bulkRead({ + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, + [](const int16_t xyz, const sensor_real_t timeDelta) {}, + }); + } + } + + std::tuple eatSamplesReturnLast( + const uint32_t milliseconds + ) { + RawVectorT accel = {0}; + RawVectorT gyro = {0}; + int16_t temp = 0; + const auto targetDelay = millis() + milliseconds; + while (millis() < targetDelay) { + sensor.bulkRead({ + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + accel[0] = xyz[0]; + accel[1] = xyz[1]; + accel[2] = xyz[2]; + }, + [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { + gyro[0] = xyz[0]; + gyro[1] = xyz[1]; + gyro[2] = xyz[2]; + }, + [&](const int16_t rawTemp, const sensor_real_t timeDelta) { + temp = rawTemp; + }, + }); + yield(); + } + return std::make_tuple(accel, gyro, temp); + } + + void checkStartupCalibration() final { + auto lastRawSample = eatSamplesReturnLast(1000); + auto gravity = static_cast( + Consts::AScale * static_cast(std::get<0>(lastRawSample)[2]) + ); + logger.info("Gravity read: %.1f (need < -7.5 to start calibration)", gravity); + if (gravity > -7.5f) { + return; + } + + ledManager.on(); + logger.info("Flip front in 5 seconds to start calibration"); + lastRawSample = eatSamplesReturnLast(5000); + gravity = static_cast( + Consts::AScale * static_cast(std::get<0>(lastRawSample)[2]) + ); + if (gravity > 7.5f) { + logger.debug("Starting calibration..."); + startCalibration(0); + } else { + logger.info("Flip not detected. Skipping calibration."); + } + + ledManager.off(); + } + + void startCalibration(int calibrationType) final { if (calibrationType == 0) { // ALL - calibrateSampleRate(eatSamplesForSeconds); + calibrateSampleRate(); if constexpr (Base::HasMotionlessCalib) { typename IMU::MotionlessCalibrationData calibData; sensor.motionlessCalibration(calibData); @@ -73,14 +145,14 @@ class SoftfusionCalibrator : public CalibrationBase // Gryoscope offset calibration can only happen after any motionless // gyroscope calibration, otherwise we are calculating the offset based // on an incorrect starting point - calibrateGyroOffset(eatSamplesReturnLast); - calibrateAccel(eatSamplesForSeconds); + calibrateGyroOffset(); + calibrateAccel(); } else if (calibrationType == 1) { - calibrateSampleRate(eatSamplesForSeconds); + calibrateSampleRate(); } else if (calibrationType == 2) { - calibrateGyroOffset(eatSamplesReturnLast); + calibrateGyroOffset(); } else if (calibrationType == 3) { - calibrateAccel(eatSamplesForSeconds); + calibrateAccel(); } else if (calibrationType == 4) { if constexpr (Base::HasMotionlessCalib) { typename IMU::MotionlessCalibrationData calibData; @@ -133,28 +205,28 @@ class SoftfusionCalibrator : public CalibrationBase accelSample[0] = (calibration.A_Ainv[0][0] * tmp[0] + calibration.A_Ainv[0][1] * tmp[1] + calibration.A_Ainv[0][2] * tmp[2]) - * AScale; + * Consts::AScale; accelSample[1] = (calibration.A_Ainv[1][0] * tmp[0] + calibration.A_Ainv[1][1] * tmp[1] + calibration.A_Ainv[1][2] * tmp[2]) - * AScale; + * Consts::AScale; accelSample[2] = (calibration.A_Ainv[2][0] * tmp[0] + calibration.A_Ainv[2][1] * tmp[1] + calibration.A_Ainv[2][2] * tmp[2]) - * AScale; + * Consts::AScale; } float getAccelTimestep() final { return calibration.A_Ts; } void scaleGyroSample(sensor_real_t gyroSample[3]) final { gyroSample[0] = static_cast( - GScale * (gyroSample[0] - calibration.G_off[0]) + Consts::GScale * (gyroSample[0] - calibration.G_off[0]) ); gyroSample[1] = static_cast( - GScale * (gyroSample[1] - calibration.G_off[1]) + Consts::GScale * (gyroSample[1] - calibration.G_off[1]) ); gyroSample[2] = static_cast( - GScale * (gyroSample[2] - calibration.G_off[2]) + Consts::GScale * (gyroSample[2] - calibration.G_off[2]) ); } @@ -185,14 +257,15 @@ class SoftfusionCalibrator : public CalibrationBase configuration.save(); } - void calibrateGyroOffset(const Base::ReturnLastFn& eatSamplesReturnLast) { + void calibrateGyroOffset() { if (!toggles.getToggle(SensorToggles::CalibrationEnabled)) { return; } // Wait for sensor to calm down before calibration logger.info( - "Put down the device and wait for baseline gyro reading calibration (%d " + "Put down the device and wait for baseline gyro reading calibration " + "(%d " "seconds)", GyroCalibDelaySeconds ); @@ -216,7 +289,7 @@ class SoftfusionCalibrator : public CalibrationBase #ifdef ESP8266 ESP.wdtFeed(); #endif - sensor.bulkRead( + sensor.bulkRead({ [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, [&sumXYZ, &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) { @@ -225,8 +298,8 @@ class SoftfusionCalibrator : public CalibrationBase sumXYZ[2] += xyz[2]; ++sampleCount; }, - [](const int16_t rawTemp, const sensor_real_t timeDelta) {} - ); + [](const int16_t rawTemp, const sensor_real_t timeDelta) {}, + }); } ledManager.off(); @@ -244,14 +317,15 @@ class SoftfusionCalibrator : public CalibrationBase ); } - void calibrateAccel(const Base::EatSamplesFn& eatSamplesForSeconds) { + void calibrateAccel() { if (!toggles.getToggle(SensorToggles::CalibrationEnabled)) { return; } auto magneto = std::make_unique(); logger.info( - "Put the device into 6 unique orientations (all sides), leave it still and " + "Put the device into 6 unique orientations (all sides), leave it still " + "and " "do not hold/touch for %d seconds each", AccelCalibRestSeconds ); @@ -290,17 +364,17 @@ class SoftfusionCalibrator : public CalibrationBase #ifdef ESP8266 ESP.wdtFeed(); #endif - sensor.bulkRead( + sensor.bulkRead({ [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { const sensor_real_t scaledData[] = {static_cast( - AScale * static_cast(xyz[0]) + Consts::AScale * static_cast(xyz[0]) ), static_cast( - AScale * static_cast(xyz[1]) + Consts::AScale * static_cast(xyz[1]) ), static_cast( - AScale * static_cast(xyz[2]) + Consts::AScale * static_cast(xyz[2]) )}; calibrationRestDetection.updateAcc(IMU::AccTs, scaledData); @@ -347,8 +421,8 @@ class SoftfusionCalibrator : public CalibrationBase } }, [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, - [](const int16_t rawTemp, const sensor_real_t timeDelta) {} - ); + [](const int16_t rawTemp, const sensor_real_t timeDelta) {}, + }); } ledManager.off(); logger.debug("Calculating accelerometer calibration data..."); @@ -376,7 +450,7 @@ class SoftfusionCalibrator : public CalibrationBase logger.debug("}"); } - void calibrateSampleRate(const Base::EatSamplesFn& eatSamplesForSeconds) { + void calibrateSampleRate() { logger.debug( "Calibrating IMU sample rate in %d second(s)...", SampleRateCalibDelaySeconds @@ -392,7 +466,7 @@ class SoftfusionCalibrator : public CalibrationBase logger.debug("Counting samples now..."); uint32_t currentTime; while ((currentTime = millis()) < calibTarget) { - sensor.bulkRead( + sensor.bulkRead({ [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, @@ -401,8 +475,8 @@ class SoftfusionCalibrator : public CalibrationBase }, [&](const int16_t rawTemp, const sensor_real_t timeDelta) { tempSamples++; - } - ); + }, + }); yield(); } @@ -423,7 +497,8 @@ class SoftfusionCalibrator : public CalibrationBase = millisFromStart / (static_cast(tempSamples) * 1000.0f); logger.debug( - "Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: %fHz", + "Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: " + "%fHz", 1.0 / calibration.G_Ts, 1.0 / calibration.A_Ts, 1.0 / calibration.T_Ts @@ -453,12 +528,10 @@ class SoftfusionCalibrator : public CalibrationBase }; private: - using Base::AScale; - using Base::GScale; using Base::logger; using Base::sensor; using Base::sensorId; using Base::toggles; }; -} // namespace SlimeVR::Sensor +} // namespace SlimeVR::Sensors diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index ab139c054..ee159eb25 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -30,6 +30,7 @@ #include #include "bmi270fw.h" +#include "callbacks.h" #include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -410,12 +411,7 @@ struct BMI270 { return to_ret; } - template - void bulkRead( - AccelCall&& processAccelSample, - GyroCall&& processGyroSample, - TempCall&& processTempSample - ) { + void bulkRead(DriverCallbacks&& callbacks) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); const auto bytes_to_read = std::min( @@ -456,7 +452,7 @@ struct BMI270 { static_cast(ShortLimit::min()), static_cast(ShortLimit::max()) ); - processGyroSample(gyro, GyrTs); + callbacks.processGyroSample(gyro, GyrTs); } if (header & Fifo::AccelDataBit) { @@ -464,7 +460,7 @@ struct BMI270 { accel[0] = getFromFifo(i, read_buffer); accel[1] = getFromFifo(i, read_buffer); accel[2] = getFromFifo(i, read_buffer); - processAccelSample(accel, AccTs); + callbacks.processAccelSample(accel, AccTs); } } } diff --git a/src/sensors/softfusion/drivers/callbacks.h b/src/sensors/softfusion/drivers/callbacks.h new file mode 100644 index 000000000..79182dc2b --- /dev/null +++ b/src/sensors/softfusion/drivers/callbacks.h @@ -0,0 +1,34 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & 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 + +template +struct DriverCallbacks { + std::function processAccelSample; + std::function processGyroSample; + std::function processTempSample; +}; diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index 767607255..6f8945d44 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -27,6 +27,7 @@ #include #include +#include "callbacks.h" #include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -157,12 +158,7 @@ struct ICM42688 { return true; } - template - void bulkRead( - AccelCall&& processAccelSample, - GyroCall&& processGyroSample, - TempCall&& processTemperatureSample - ) { + void bulkRead(DriverCallbacks&& callbacks) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); std::array read_buffer; // max 8 readings @@ -185,7 +181,7 @@ struct ICM42688 { static_cast(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf), static_cast(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf), }; - processGyroSample(gyroData, GyrTs); + callbacks.processGyroSample(gyroData, GyrTs); if (entry.part.accel[0] != -32768) { const int32_t accelData[3]{ @@ -196,11 +192,14 @@ struct ICM42688 { static_cast(entry.part.accel[2]) << 4 | (static_cast(entry.part.zlsb) & 0xf0 >> 4), }; - processAccelSample(accelData, AccTs); + callbacks.processAccelSample(accelData, AccTs); } if (entry.part.temp != 0x8000) { - processTemperatureSample(static_cast(entry.part.temp), TempTs); + callbacks.processTempSample( + static_cast(entry.part.temp), + TempTs + ); } } } diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 907daf88c..ca4bcf130 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -24,6 +24,8 @@ #include #include +#include "callbacks.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 32g @@ -135,12 +137,7 @@ struct ICM45Base { return true; } - template - void bulkRead( - AccelCall&& processAccelSample, - GyroCall&& processGyroSample, - TempCall&& processTemperatureSample - ) { + void bulkRead(DriverCallbacks&& callbacks) { // Allocate statically so that it does not take up stack space, which // can result in stack overflow and panic constexpr size_t MaxReadings = 8; @@ -198,7 +195,7 @@ struct ICM45Base { static_cast(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf), static_cast(entry.gyro[2]) << 4 | (entry.lsb[2] & 0xf), }; - processGyroSample(gyroData, GyrTs); + callbacks.processGyroSample(gyroData, GyrTs); } if (has_accel && entry.accel[0] != InvalidReading) { @@ -210,11 +207,11 @@ struct ICM45Base { static_cast(entry.accel[2]) << 4 | (static_cast((entry.lsb[2]) & 0xf0) >> 4), }; - processAccelSample(accelData, AccTs); + callbacks.processAccelSample(accelData, AccTs); } if (entry.temp != 0x8000) { - processTemperatureSample(static_cast(entry.temp), TempTs); + callbacks.processTempSample(static_cast(entry.temp), TempTs); } } } diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 24c617734..3f5008b1b 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -27,7 +27,7 @@ #include #include -namespace SlimeVR::Sensors::SoftFusion::Drivers { +#include "callbacks.h" template struct LSM6DSOutputHandler { @@ -49,11 +49,9 @@ struct LSM6DSOutputHandler { static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; - template + template void bulkRead( - AccelCall& processAccelSample, - GyroCall& processGyroSample, - TempCall& processTempSample, + DriverCallbacks&& callbacks, float GyrTs, float AccTs, float TempTs @@ -88,17 +86,15 @@ struct LSM6DSOutputHandler { switch (tag) { case 0x01: // Gyro NC - processGyroSample(entry.xyz, GyrTs); + callbacks.processGyroSample(entry.xyz, GyrTs); break; case 0x02: // Accel NC - processAccelSample(entry.xyz, AccTs); + callbacks.processAccelSample(entry.xyz, AccTs); break; case 0x03: // Temperature - processTempSample(entry.xyz[0], TempTs); + callbacks.processTempSample(entry.xyz[0], TempTs); break; } } } }; - -} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index e2e44e6a5..ebaf71f14 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -27,6 +27,7 @@ #include #include +#include "callbacks.h" #include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -122,12 +123,7 @@ struct LSM6DS3TRC { return true; } - template - void bulkRead( - AccelCall&& processAccelSample, - GyroCall&& processGyroSample, - TempCall&& processTemperatureSample - ) { + void bulkRead(DriverCallbacks&& callbacks) { const auto read_result = i2c.readReg16(Regs::FifoStatus); if (read_result & 0x4000) { // overrun! // disable and re-enable fifo to clear it @@ -157,12 +153,15 @@ struct LSM6DS3TRC { ); for (uint16_t i = 0; i < bytes_to_read / sizeof(uint16_t); i += single_measurement_words) { - processGyroSample(reinterpret_cast(&read_buffer[i]), GyrTs); - processAccelSample( + callbacks.processGyroSample( + reinterpret_cast(&read_buffer[i]), + GyrTs + ); + callbacks.processAccelSample( reinterpret_cast(&read_buffer[i + 3]), AccTs ); - processTemperatureSample(read_buffer[i + 9], TempTs); + callbacks.processTempSample(read_buffer[i + 9], TempTs); } } }; diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 17d17bc2d..f113471e2 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -27,6 +27,7 @@ #include #include +#include "callbacks.h" #include "lsm6ds-common.h" #include "vqf.h" @@ -119,21 +120,13 @@ struct LSM6DSO : LSM6DSOutputHandler { return true; } - template - void bulkRead( - AccelCall&& processAccelSample, - GyroCall&& processGyroSample, - TempCall&& processTempSample - ) { - LSM6DSOutputHandler:: - template bulkRead( - processAccelSample, - processGyroSample, - processTempSample, - GyrTs, - AccTs, - TempTs - ); + void bulkRead(DriverCallbacks&& callbacks) { + LSM6DSOutputHandler::template bulkRead( + std::move(callbacks), + GyrTs, + AccTs, + TempTs + ); } }; diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index a9d7ac1da..0dcd5f7a1 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -118,21 +118,13 @@ struct LSM6DSR : LSM6DSOutputHandler { return true; } - template - void bulkRead( - AccelCall&& processAccelSample, - GyroCall&& processGyroSample, - TempCall&& processTempSample - ) { - LSM6DSOutputHandler:: - template bulkRead( - processAccelSample, - processGyroSample, - processTempSample, - GyrTs, - AccTs, - TempTs - ); + void bulkRead(DriverCallbacks&& callbacks) { + LSM6DSOutputHandler::template bulkRead( + std::move(callbacks), + GyrTs, + AccTs, + TempTs + ); } }; diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index 60c9a9bfc..796ba6be4 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -134,21 +134,13 @@ struct LSM6DSV : LSM6DSOutputHandler { return true; } - template - void bulkRead( - AccelCall&& processAccelSample, - GyroCall&& processGyroSample, - TempCall&& processTempSample - ) { - LSM6DSOutputHandler:: - template bulkRead( - processAccelSample, - processGyroSample, - processTempSample, - GyrTs, - AccTs, - TempTs - ); + void bulkRead(DriverCallbacks&& callbacks) { + LSM6DSOutputHandler::template bulkRead( + std::move(callbacks), + GyrTs, + AccTs, + TempTs + ); } }; diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 79c24ba60..feeb158ec 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -29,6 +29,7 @@ #include #include +#include "callbacks.h" #include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -178,8 +179,7 @@ struct MPU6050 { return result; } - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { + void bulkRead(DriverCallbacks&& callbacks) { const auto status = i2c.readReg(Regs::IntStatus); if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { @@ -209,12 +209,12 @@ struct MPU6050 { xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x); xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y); xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z); - processAccelSample(xyz, AccTs); + callbacks.processAccelSample(xyz, AccTs); xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x); xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y); xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z); - processGyroSample(xyz, GyrTs); + callbacks.processGyroSample(xyz, GyrTs); } } }; diff --git a/src/sensors/softfusion/imuconsts.h b/src/sensors/softfusion/imuconsts.h new file mode 100644 index 000000000..7def220fe --- /dev/null +++ b/src/sensors/softfusion/imuconsts.h @@ -0,0 +1,56 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & 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 "../../motionprocessing/types.h" + +template +struct IMUConsts { + static constexpr bool Uses32BitSensorData + = requires(IMU& i) { i.Uses32BitSensorData; }; + + static constexpr bool DirectTempReadOnly = requires(IMU& 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; + + static constexpr float DirectTempReadFreq = 15; + static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq; + static constexpr sensor_real_t getDefaultTempTs() { + if constexpr (DirectTempReadOnly) { + return DirectTempReadTs; + } else { + return IMU::TempTs; + } + } +}; diff --git a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h index e891da85e..b7a989bec 100644 --- a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h +++ b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h @@ -30,6 +30,7 @@ #include "../../../GlobalVars.h" #include "../../../configuration/Configuration.h" #include "../../SensorFusionRestDetect.h" +#include "../CalibrationBase.h" #include "AccelBiasCalibrationStep.h" #include "GyroBiasCalibrationStep.h" #include "MotionlessCalibrationStep.h" @@ -37,31 +38,30 @@ #include "SampleRateCalibrationStep.h" #include "configuration/SensorConfig.h" #include "logging/Logger.h" -#include "sensors/softfusion/CalibrationBase.h" namespace SlimeVR::Sensors::RuntimeCalibration { -template -class RuntimeCalibrator : public Sensor::CalibrationBase { +template +class RuntimeCalibrator : public Sensors::CalibrationBase { public: static constexpr bool HasUpsideDownCalibration = false; - using Base = Sensor::CalibrationBase; - using Self = RuntimeCalibrator; + using Base = Sensors::CalibrationBase; + using Self = RuntimeCalibrator; + using Consts = typename Base::Consts; + using RawSensorT = typename Consts::RawSensorT; + using RawVectorT = typename Consts::RawVectorT; RuntimeCalibrator( SensorFusionRestDetect& fusion, IMU& imu, uint8_t sensorId, Logging::Logger& logger, - float TempTs, - float AScale, - float GScale, SensorToggleState& toggles ) - : Base{fusion, imu, sensorId, logger, TempTs, AScale, GScale, toggles} { - calibration.T_Ts = TempTs; - activeCalibration.T_Ts = TempTs; + : Base{fusion, imu, sensorId, logger, toggles} { + calibration.T_Ts = Consts::TempTs; + activeCalibration.T_Ts = Consts::TempTs; } bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration @@ -152,22 +152,22 @@ class RuntimeCalibrator : public Sensor::CalibrationBase( - GScale * (gyroSample[0] - activeCalibration.G_off1[0]) + Consts::GScale * (gyroSample[0] - activeCalibration.G_off1[0]) ); gyroSample[1] = static_cast( - GScale * (gyroSample[1] - activeCalibration.G_off1[1]) + Consts::GScale * (gyroSample[1] - activeCalibration.G_off1[1]) ); gyroSample[2] = static_cast( - GScale * (gyroSample[2] - activeCalibration.G_off1[2]) + Consts::GScale * (gyroSample[2] - activeCalibration.G_off1[2]) ); } @@ -202,12 +202,12 @@ class RuntimeCalibrator : public Sensor::CalibrationBase sampleRateCalibrationStep{calibration}; MotionlessCalibrationStep motionlessCalibrationStep{ calibration, - sensor - }; + sensor}; GyroBiasCalibrationStep gyroBiasCalibrationStep{calibration}; AccelBiasCalibrationStep accelBiasCalibrationStep{ calibration, - static_cast(Base::AScale) - }; + static_cast(Consts::AScale)}; NullCalibrationStep nullCalibrationStep{calibration}; CalibrationStep* currentStep = &nullCalibrationStep; @@ -440,9 +438,7 @@ class RuntimeCalibrator : public Sensor::CalibrationBase + #include #include +#include +#include #include +#include "../../GlobalVars.h" +#include "../../sensorinterface/SensorInterface.h" #include "../RestCalibrationDetector.h" #include "../SensorFusionRestDetect.h" #include "../sensor.h" -#include "GlobalVars.h" +#include "TempGradientCalculator.h" +#include "drivers/callbacks.h" +#include "imuconsts.h" #include "motionprocessing/types.h" -#include "sensors/softfusion/TempGradientCalculator.h" namespace SlimeVR::Sensors { @@ -40,57 +47,37 @@ template < template typename T, typename I2CImpl, - template + template typename Calibrator> class SoftFusionSensor : public Sensor { - using imu = T; - - static constexpr sensor_real_t getDefaultTempTs() { - if constexpr (DirectTempReadOnly) { - return DirectTempReadTs; - } else { - return imu::TempTs; - } - } - - static constexpr bool Uses32BitSensorData - = requires(imu& i) { i.Uses32BitSensorData; }; - - static constexpr bool DirectTempReadOnly = requires(imu& 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; + using IMU = T; + using Self = SoftFusionSensor; - using Calib = Calibrator; + using Consts = IMUConsts; + using RawSensorT = typename Consts::RawSensorT; + using Calib = Calibrator; static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration; - static constexpr float DirectTempReadFreq = 15; - static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq; float lastReadTemperature = 0; uint32_t lastTempPollTime = micros(); bool detected() const { - const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg); - if (imu::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, - value - ); - return false; + const auto value = m_sensor.i2c.readReg(IMU::Regs::WhoAmI::reg); + if (IMU::Regs::WhoAmI::value == value) { + return true; } - return true; + m_Logger.error( + "Sensor not detected, expected reg 0x%02x = 0x%02x but got 0x%02x", + IMU::Regs::WhoAmI::reg, + IMU::Regs::WhoAmI::value, + value + ); + return false; } - void sendData() { + void sendData() final { Sensor::sendData(); sendTempIfNeeded(); } @@ -138,10 +125,10 @@ class SoftFusionSensor : public Sensor { void processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) { - if constexpr (!DirectTempReadOnly) { - const float scaledTemperature = imu::TemperatureBias + if constexpr (!Consts::DirectTempReadOnly) { + const float scaledTemperature = IMU::TemperatureBias + static_cast(rawTemperature) - * (1.0 / imu::TemperatureSensitivity); + * (1.0 / IMU::TemperatureSensitivity); lastReadTemperature = scaledTemperature; if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) { @@ -155,57 +142,9 @@ class SoftFusionSensor : public Sensor { } } - void eatSamplesForSeconds(const uint32_t seconds) { - const auto targetDelay = millis() + 1000 * seconds; - auto lastSecondsRemaining = seconds; - while (millis() < targetDelay) { -#ifdef ESP8266 - ESP.wdtFeed(); -#endif - auto currentSecondsRemaining = (targetDelay - millis()) / 1000; - if (currentSecondsRemaining != lastSecondsRemaining) { - m_Logger.info("%d...", currentSecondsRemaining + 1); - lastSecondsRemaining = currentSecondsRemaining; - } - m_sensor.bulkRead( - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, - [](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}, - [](const int16_t xyz, const sensor_real_t timeDelta) {} - ); - } - } - - std::tuple eatSamplesReturnLast( - const uint32_t milliseconds - ) { - RawVectorT accel = {0}; - RawVectorT gyro = {0}; - int16_t temp = 0; - const auto targetDelay = millis() + milliseconds; - while (millis() < targetDelay) { - m_sensor.bulkRead( - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - accel[0] = xyz[0]; - accel[1] = xyz[1]; - accel[2] = xyz[2]; - }, - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - gyro[0] = xyz[0]; - gyro[1] = xyz[1]; - gyro[2] = xyz[2]; - }, - [&](const int16_t rawTemp, const sensor_real_t timeDelta) { - temp = rawTemp; - } - ); - yield(); - } - return std::make_tuple(accel, gyro, temp); - } - public: - static constexpr auto TypeID = imu::Type; - static constexpr uint8_t Address = imu::Address; + static constexpr auto TypeID = IMU::Type; + static constexpr uint8_t Address = IMU::Address; SoftFusionSensor( uint8_t id, @@ -215,8 +154,8 @@ class SoftFusionSensor : public Sensor { 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) + : 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) {} ~SoftFusionSensor() override = default; @@ -246,12 +185,13 @@ class SoftFusionSensor : public Sensor { // read fifo updating fusion uint32_t now = micros(); - if constexpr (DirectTempReadOnly) { + if constexpr (Consts::DirectTempReadOnly) { uint32_t tempElapsed = now - lastTempPollTime; - if (tempElapsed >= DirectTempReadTs * 1e6) { + if (tempElapsed >= Consts::DirectTempReadTs * 1e6) { lastTempPollTime = now - - (tempElapsed - static_cast(DirectTempReadTs * 1e6)); + - (tempElapsed + - static_cast(Consts::DirectTempReadTs * 1e6)); lastReadTemperature = m_sensor.getDirectTemp(); calibrator.provideTempSample(lastReadTemperature); @@ -259,7 +199,7 @@ class SoftFusionSensor : public Sensor { if (toggles.getToggle(SensorToggles::TempGradientCalibrationEnabled)) { tempGradientCalculator.feedSample( lastReadTemperature, - DirectTempReadTs + Consts::DirectTempReadTs ); } } @@ -278,20 +218,20 @@ class SoftFusionSensor : public Sensor { // send new fusion values when time is up now = micros(); constexpr float maxSendRateHz = 100.0f; - constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6; + constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6f; elapsed = now - m_lastRotationPacketSent; if (elapsed >= sendInterval) { - m_sensor.bulkRead( - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - processAccelSample(xyz, timeDelta); + m_sensor.bulkRead({ + [&](const auto sample[3], float AccTs) { + processAccelSample(sample, AccTs); }, - [&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { - processGyroSample(xyz, timeDelta); + [&](const auto sample[3], float GyrTs) { + processGyroSample(sample, GyrTs); }, - [&](const int16_t rawTemp, const sensor_real_t timeDelta) { - processTempSample(rawTemp, timeDelta); - } - ); + [&](int16_t sample, float TempTs) { + processTempSample(sample, TempTs); + }, + }); if (!m_fusion.isUpdated()) { checkSensorTimeout(); return; @@ -346,7 +286,7 @@ class SoftFusionSensor : public Sensor { bool initResult = false; if constexpr (Calib::HasMotionlessCalib) { - typename imu::MotionlessCalibrationData calibData; + typename IMU::MotionlessCalibrationData calibData; std::memcpy( &calibData, calibrator.getMotionlessCalibrationData(), @@ -365,43 +305,15 @@ class SoftFusionSensor : public Sensor { m_status = SensorStatus::SENSOR_OK; working = true; - [[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000); - if constexpr (UpsideDownCalibrationInit) { - auto gravity = static_cast( - AScale * static_cast(std::get<0>(lastRawSample)[2]) - ); - m_Logger.info( - "Gravity read: %.1f (need < -7.5 to start calibration)", - gravity - ); - if (gravity < -7.5f) { - ledManager.on(); - m_Logger.info("Flip front in 5 seconds to start calibration"); - lastRawSample = eatSamplesReturnLast(5000); - gravity = static_cast( - AScale * static_cast(std::get<0>(lastRawSample)[2]) - ); - if (gravity > 7.5f) { - m_Logger.debug("Starting calibration..."); - startCalibration(0); - } else { - m_Logger.info("Flip not detected. Skipping calibration."); - } - ledManager.off(); - } - } + calibrator.checkStartupCalibration(); } void startCalibration(int calibrationType) final { - calibrator.startCalibration( - calibrationType, - [&](const uint32_t seconds) { eatSamplesForSeconds(seconds); }, - [&](const uint32_t millis) { return eatSamplesReturnLast(millis); } - ); + calibrator.startCalibration(calibrationType); } - bool isFlagSupported(SensorToggles toggle) const final { + [[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final { return toggle == SensorToggles::CalibrationEnabled || toggle == SensorToggles::TempGradientCalibrationEnabled; } @@ -409,17 +321,8 @@ class SoftFusionSensor : public Sensor { SensorStatus getSensorState() final { return m_status; } SensorFusionRestDetect m_fusion; - imu m_sensor; - Calib calibrator{ - m_fusion, - m_sensor, - sensorId, - m_Logger, - getDefaultTempTs(), - AScale, - GScale, - toggles - }; + IMU m_sensor; + Calib calibrator{m_fusion, m_sensor, sensorId, m_Logger, toggles}; SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; uint32_t m_lastPollTime = micros(); From 4b65c66a52055c9b13a60baf0b30f9d8697cb4ec Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:14:02 +0200 Subject: [PATCH 05/37] Missed stuff --- src/sensors/softfusion/softfusionsensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index a7ea78e08..47face8ed 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -151,7 +151,7 @@ class SoftFusionSensor : public Sensor { uint8_t i2cAddress, float rotation, SlimeVR::SensorInterface* sensorInterface, - PinInterface* intPin = nullptr, + PinInterface* = nullptr, uint8_t = 0 ) : Sensor(IMU::Name, IMU::Type, id, i2cAddress, rotation, sensorInterface) From 50a2da324027939f4380e02027cb4bd61cd6a012 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:14:30 +0200 Subject: [PATCH 06/37] Undo defines changes --- src/defines.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/defines.h b/src/defines.h index 833245cf6..f1c5ecc15 100644 --- a/src/defines.h +++ b/src/defines.h @@ -26,7 +26,7 @@ // ================================================ // Set parameters of IMU and board used -#define IMU IMU_LSM6DSV +#define IMU IMU_BNO085 #define SECOND_IMU IMU #define BOARD BOARD_SLIMEVR #define IMU_ROTATION DEG_270 From e964032f4fe5c9c8aa0ebe0616fee3a4d0e7e48c Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:18:58 +0200 Subject: [PATCH 07/37] Undo debug.h changes --- src/debug.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/debug.h b/src/debug.h index 55c678498..cf49ea4ff 100644 --- a/src/debug.h +++ b/src/debug.h @@ -101,7 +101,7 @@ #endif #ifndef USE_RUNTIME_CALIBRATION -#define USE_RUNTIME_CALIBRATION false +#define USE_RUNTIME_CALIBRATION true #endif #define DEBUG_MEASURE_SENSOR_TIME_TAKEN false From 7dc88afd26e7942e73e66fb316eaba7537cdaa80 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:25:01 +0200 Subject: [PATCH 08/37] Uses32BitSensorData is unneccessary now --- src/sensors/softfusion/SoftfusionCalibration.h | 2 +- src/sensors/softfusion/drivers/icm42688.h | 2 -- src/sensors/softfusion/drivers/icm45base.h | 2 -- src/sensors/softfusion/imuconsts.h | 5 ++++- .../softfusion/runtimecalibration/RuntimeCalibration.h | 4 ++-- 5 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h index 6c47bf97f..831df7de3 100644 --- a/src/sensors/softfusion/SoftfusionCalibration.h +++ b/src/sensors/softfusion/SoftfusionCalibration.h @@ -55,7 +55,7 @@ class SoftfusionCalibrator : public CalibrationBase { SensorToggleState& toggles ) : Base{fusion, sensor, sensorId, logger, toggles} { - calibration.T_Ts = IMU::TempTs; + calibration.T_Ts = Consts::getDefaultTempTs(); } void eatSamplesForSeconds(const uint32_t seconds) { diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index 6f8945d44..d7cd1add1 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -52,8 +52,6 @@ struct ICM42688 { static constexpr float GyroSensitivity = 32.8f; static constexpr float AccelSensitivity = 4096.0f; - static constexpr bool Uses32BitSensorData = true; - static constexpr float TemperatureBias = 25.0f; static constexpr float TemperatureSensitivity = 2.07f; diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index ca4bcf130..cdb64f721 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -53,8 +53,6 @@ struct ICM45Base { static constexpr float TemperatureZROChange = 20.0f; - static constexpr bool Uses32BitSensorData = true; - I2CImpl i2c; SlimeVR::Logging::Logger& logger; ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) diff --git a/src/sensors/softfusion/imuconsts.h b/src/sensors/softfusion/imuconsts.h index 7def220fe..3ef880045 100644 --- a/src/sensors/softfusion/imuconsts.h +++ b/src/sensors/softfusion/imuconsts.h @@ -28,11 +28,14 @@ #include #include "../../motionprocessing/types.h" +#include "drivers/callbacks.h" template struct IMUConsts { static constexpr bool Uses32BitSensorData - = requires(IMU& i) { i.Uses32BitSensorData; }; + = requires(IMU& i, DriverCallbacks callbacks) { + i.bulkRead(std::move(callbacks)); + }; static constexpr bool DirectTempReadOnly = requires(IMU& i) { i.getDirectTemp(); }; diff --git a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h index b7a989bec..91a8022e2 100644 --- a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h +++ b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h @@ -60,8 +60,8 @@ class RuntimeCalibrator : public Sensors::CalibrationBase { SensorToggleState& toggles ) : Base{fusion, imu, sensorId, logger, toggles} { - calibration.T_Ts = Consts::TempTs; - activeCalibration.T_Ts = Consts::TempTs; + calibration.T_Ts = Consts::getDefaultTempTs(); + activeCalibration.T_Ts = Consts::getDefaultTempTs(); } bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration From e737ebadc89efeca65a2fcf7e5e8d3febb870621 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:26:57 +0200 Subject: [PATCH 09/37] Remove some unnecessary variables --- src/sensors/softfusion/CalibrationBase.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/sensors/softfusion/CalibrationBase.h b/src/sensors/softfusion/CalibrationBase.h index 91ae20d8e..ee23ee0b5 100644 --- a/src/sensors/softfusion/CalibrationBase.h +++ b/src/sensors/softfusion/CalibrationBase.h @@ -52,7 +52,6 @@ class CalibrationBase { using Consts = IMUConsts; using RawSensorT = typename Consts::RawSensorT; - using RawVectorT = typename Consts::RawVectorT; static constexpr bool HasMotionlessCalib = requires(IMU& i) { typename IMU::MotionlessCalibrationData; }; @@ -64,12 +63,7 @@ class CalibrationBase { } } - using EatSamplesFn = std::function; - using ReturnLastFn - = std::function(const uint32_t)>; - virtual void checkStartupCalibration() {} - virtual void startCalibration(int calibrationType){}; virtual bool calibrationMatches( From 6ef997a8898cdb08d94441d7189804d410def77d Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:28:56 +0200 Subject: [PATCH 10/37] Cleanup some logging text --- src/sensors/softfusion/SoftfusionCalibration.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h index 831df7de3..010955fb4 100644 --- a/src/sensors/softfusion/SoftfusionCalibration.h +++ b/src/sensors/softfusion/SoftfusionCalibration.h @@ -265,8 +265,7 @@ class SoftfusionCalibrator : public CalibrationBase { // Wait for sensor to calm down before calibration logger.info( "Put down the device and wait for baseline gyro reading calibration " - "(%d " - "seconds)", + "(%d seconds)", GyroCalibDelaySeconds ); ledManager.on(); @@ -325,8 +324,7 @@ class SoftfusionCalibrator : public CalibrationBase { auto magneto = std::make_unique(); logger.info( "Put the device into 6 unique orientations (all sides), leave it still " - "and " - "do not hold/touch for %d seconds each", + "and do not hold/touch for %d seconds each", AccelCalibRestSeconds ); ledManager.on(); From 2778185657733d6e0f0ebe7d5c8569772f4fa669 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 23 Apr 2025 23:32:08 +0200 Subject: [PATCH 11/37] Formatting --- .../softfusion/runtimecalibration/RuntimeCalibration.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h index 91a8022e2..947561baf 100644 --- a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h +++ b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h @@ -397,11 +397,13 @@ class RuntimeCalibrator : public Sensors::CalibrationBase { SampleRateCalibrationStep sampleRateCalibrationStep{calibration}; MotionlessCalibrationStep motionlessCalibrationStep{ calibration, - sensor}; + sensor + }; GyroBiasCalibrationStep gyroBiasCalibrationStep{calibration}; AccelBiasCalibrationStep accelBiasCalibrationStep{ calibration, - static_cast(Consts::AScale)}; + static_cast(Consts::AScale) + }; NullCalibrationStep nullCalibrationStep{calibration}; CalibrationStep* currentStep = &nullCalibrationStep; From f3e55191ef7f2697c3c2f5bb2e61850f5384f8f4 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 03:03:36 +0200 Subject: [PATCH 12/37] Split SensorBuilder into a header-source file pair --- src/sensors/SensorBuilder.cpp | 310 ++++++++++++++++++++++++++++++++++ src/sensors/SensorBuilder.h | 255 +--------------------------- 2 files changed, 319 insertions(+), 246 deletions(-) create mode 100644 src/sensors/SensorBuilder.cpp diff --git a/src/sensors/SensorBuilder.cpp b/src/sensors/SensorBuilder.cpp new file mode 100644 index 000000000..407768559 --- /dev/null +++ b/src/sensors/SensorBuilder.cpp @@ -0,0 +1,310 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Eiren Rain, Gorbit99 & 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. +*/ + +#include "SensorBuilder.h" + +namespace SlimeVR::Sensors { + +SensorBuilder::SensorBuilder(SensorManager* sensorManager) + : m_Manager(sensorManager) {} + +uint8_t SensorBuilder::buildAllSensors() { + SensorInterfaceManager interfaceManager; + + uint8_t sensorID = 0; + uint8_t activeSensorCount = 0; + +#define NO_PIN nullptr +#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin) +#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) +#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) +#define PCA_WIRE(scl, sda, addr, ch) \ + interfaceManager.pcaWireInterface().get(scl, sda, addr, ch); +#define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ + *(new SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN)) + +#define SENSOR_DESC_ENTRY(ImuType, ...) \ + { \ + do { \ + std::unique_ptr<::Sensor> sensor; \ + if constexpr (std::is_same::value) { \ + auto sensorType = findSensorType(sensorID, __VA_ARGS__); \ + if (sensorType == SensorTypeID::Unknown) { \ + m_Manager->m_Logger.error( \ + "Can't find sensor type for sensor %d", \ + sensorID \ + ); \ + break; \ + } else { \ + m_Manager->m_Logger.info( \ + "Sensor %d automatically detected with %s", \ + sensorID, \ + getIMUNameByType(sensorType) \ + ); \ + sensor \ + = buildSensorDynamically(sensorType, sensorID, __VA_ARGS__); \ + } \ + } else { \ + sensor = buildSensor(sensorID, __VA_ARGS__); \ + } \ + if (sensor->isWorking()) { \ + m_Manager->m_Logger.info("Sensor %d configured", sensorID); \ + activeSensorCount++; \ + } \ + m_Manager->m_Sensors.push_back(std::move(sensor)); \ + } while (false); \ + sensorID++; \ + } + + // Apply descriptor list and expand to entries + SENSOR_DESC_LIST; + +#define SENSOR_INFO_ENTRY(ImuID, ...) \ + { m_Manager->m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); } + + // Apply sensor info list + SENSOR_INFO_LIST; + + return activeSensorCount; +} + +#define BUILD_SENSOR_ARGS \ + sensorID, imuInterface, rotation, sensorInterface, optional, intPin, extraParam + +std::unique_ptr<::Sensor> SensorBuilder::buildSensorDynamically( + SensorTypeID type, + uint8_t sensorID, + RegisterInterface& imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam +) { + switch (type) { + // case SensorTypeID::MPU9250: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO080: + // return buildSensor(BUILD_SENSOR_ARGS); + case SensorTypeID::BNO085: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO055: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::MPU6050: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::BNO086: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BMI160: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM20948: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM42688: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::BMI270: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DS3TRC: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::LSM6DSV: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DSO: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::LSM6DSR: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::ICM45686: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM45605: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + default: + m_Manager->m_Logger.error( + "Unable to create sensor with type %s (%d)", + getIMUNameByType(type), + type + ); + } + return std::make_unique(sensorID); +} + +std::unique_ptr<::Sensor> SensorBuilder::buildSensorDynamically( + SensorTypeID type, + uint8_t sensorID, + uint8_t imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam +) { + switch (type) { + // case SensorTypeID::MPU9250: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO080: + // return buildSensor(BUILD_SENSOR_ARGS); + case SensorTypeID::BNO085: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BNO055: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::MPU6050: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::BNO086: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::BMI160: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM20948: + // return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM42688: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::BMI270: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DS3TRC: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::LSM6DSV: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::LSM6DSO: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + // case SensorTypeID::LSM6DSR: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + case SensorTypeID::ICM45686: + return buildSensor(BUILD_SENSOR_ARGS); + // case SensorTypeID::ICM45605: + // return buildSensor( + // BUILD_SENSOR_ARGS + // ); + default: + m_Manager->m_Logger.error( + "Unable to create sensor with type %s (%d)", + getIMUNameByType(type), + type + ); + } + return std::make_unique(sensorID); +} + +SensorTypeID SensorBuilder::findSensorType( + uint8_t sensorID, + uint8_t imuAddress, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam +) { + sensorInterface->init(); + sensorInterface->swapIn(); + // if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuAddress)) + // { return SensorTypeID::LSM6DS3TRC; + // } + // if (SoftFusionICM42688::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::ICM42688; + //} + if (SoftFusionBMI270::checkPresent(sensorID, imuAddress)) { + return SensorTypeID::BMI270; + } + if (SoftFusionLSM6DSV::checkPresent(sensorID, imuAddress)) { + return SensorTypeID::LSM6DSV; + } + // if (SoftFusionLSM6DSO::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::LSM6DSO; + // } + // if (SoftFusionLSM6DSR::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::LSM6DSR; + // } + // if (SoftFusionMPU6050::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::MPU6050; + // } + if (SoftFusionICM45686::checkPresent(sensorID, imuAddress)) { + return SensorTypeID::ICM45686; + } + return BNO080Sensor::checkIfPresent(sensorID, imuAddress, intPin); + // if (SoftFusionICM45605::checkPresent(sensorID, imuAddress)) { + // return SensorTypeID::ICM45605; + // } + + return SensorTypeID::Unknown; +} + +SensorTypeID SensorBuilder::findSensorType( + uint8_t sensorID, + RegisterInterface& imuInterface, + float rotation, + SensorInterface* sensorInterface, + bool optional, + PinInterface* intPin, + int extraParam +) { + sensorInterface->init(); + sensorInterface->swapIn(); + // if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuInterface)) + // { return SensorTypeID::LSM6DS3TRC; + // } + // if (SoftFusionICM42688::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::ICM42688; + //} + if (SoftFusionBMI270::checkPresent(sensorID, imuInterface)) { + return SensorTypeID::BMI270; + } + if (SoftFusionLSM6DSV::checkPresent(sensorID, imuInterface)) { + return SensorTypeID::LSM6DSV; + } + // if (SoftFusionLSM6DSO::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::LSM6DSO; + // } + // if (SoftFusionLSM6DSR::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::LSM6DSR; + // } + // if (SoftFusionMPU6050::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::MPU6050; + // } + if (SoftFusionICM45686::checkPresent(sensorID, imuInterface)) { + return SensorTypeID::ICM45686; + } + return BNO080Sensor::checkIfPresent(sensorID, sensorInterface, intPin); + // if (SoftFusionICM45605::checkPresent(sensorID, imuInterface)) { + // return SensorTypeID::ICM45605; + // } + + return SensorTypeID::Unknown; +} + +} // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorBuilder.h b/src/sensors/SensorBuilder.h index 1a391bb8c..a874ff65f 100644 --- a/src/sensors/SensorBuilder.h +++ b/src/sensors/SensorBuilder.h @@ -29,6 +29,7 @@ #include "EmptySensor.h" #include "ErroneousSensor.h" +#include "SensorManager.h" #include "bmi160sensor.h" #include "bno055sensor.h" #include "bno080sensor.h" @@ -77,8 +78,7 @@ #include "driver/i2c.h" #endif -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { using SoftFusionLSM6DS3TRC = SoftFusionSensor; using SoftFusionICM42688 @@ -97,71 +97,9 @@ class SensorAuto {}; struct SensorBuilder { public: SensorManager* m_Manager; - SensorBuilder(SensorManager* sensorManager) - : m_Manager(sensorManager) {} + SensorBuilder(SensorManager* sensorManager); - uint8_t buildAllSensors() { - SensorInterfaceManager interfaceManager; - - uint8_t sensorID = 0; - uint8_t activeSensorCount = 0; - -#define NO_PIN nullptr -#define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin) -#define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) -#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) -#define PCA_WIRE(scl, sda, addr, ch) \ - interfaceManager.pcaWireInterface().get(scl, sda, addr, ch); -#define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ - *(new SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN)) - -#define SENSOR_DESC_ENTRY(ImuType, ...) \ - { \ - do { \ - std::unique_ptr<::Sensor> sensor; \ - if constexpr (std::is_same::value) { \ - auto sensorType = findSensorType(sensorID, __VA_ARGS__); \ - if (sensorType == SensorTypeID::Unknown) { \ - m_Manager->m_Logger.error( \ - "Can't find sensor type for sensor %d", \ - sensorID \ - ); \ - break; \ - } else { \ - m_Manager->m_Logger.info( \ - "Sensor %d automatically detected with %s", \ - sensorID, \ - getIMUNameByType(sensorType) \ - ); \ - sensor \ - = buildSensorDynamically(sensorType, sensorID, __VA_ARGS__); \ - } \ - } else { \ - sensor = buildSensor(sensorID, __VA_ARGS__); \ - } \ - if (sensor->isWorking()) { \ - m_Manager->m_Logger.info("Sensor %d configured", sensorID); \ - activeSensorCount++; \ - } \ - m_Manager->m_Sensors.push_back(std::move(sensor)); \ - } while (false); \ - sensorID++; \ - } - - // Apply descriptor list and expand to entries - SENSOR_DESC_LIST; - -#define SENSOR_INFO_ENTRY(ImuID, ...) \ - { m_Manager->m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); } - - // Apply sensor info list - SENSOR_INFO_LIST; - - return activeSensorCount; - } - -#define BUILD_SENSOR_ARGS \ - sensorID, imuInterface, rotation, sensorInterface, optional, intPin, extraParam + uint8_t buildAllSensors(); std::unique_ptr<::Sensor> buildSensorDynamically( SensorTypeID type, @@ -172,61 +110,7 @@ struct SensorBuilder { bool optional, PinInterface* intPin, int extraParam - ) { - switch (type) { - // case SensorTypeID::MPU9250: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::BNO080: - // return buildSensor(BUILD_SENSOR_ARGS); - case SensorTypeID::BNO085: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::BNO055: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::MPU6050: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - // case SensorTypeID::BNO086: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::BMI160: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::ICM20948: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::ICM42688: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - case SensorTypeID::BMI270: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::LSM6DS3TRC: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - case SensorTypeID::LSM6DSV: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::LSM6DSO: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - // case SensorTypeID::LSM6DSR: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - case SensorTypeID::ICM45686: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::ICM45605: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - default: - m_Manager->m_Logger.error( - "Unable to create sensor with type %s (%d)", - getIMUNameByType(type), - type - ); - } - return std::make_unique(sensorID); - } + ); std::unique_ptr<::Sensor> buildSensorDynamically( SensorTypeID type, @@ -237,61 +121,7 @@ struct SensorBuilder { bool optional, PinInterface* intPin, int extraParam - ) { - switch (type) { - // case SensorTypeID::MPU9250: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::BNO080: - // return buildSensor(BUILD_SENSOR_ARGS); - case SensorTypeID::BNO085: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::BNO055: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::MPU6050: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - // case SensorTypeID::BNO086: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::BMI160: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::ICM20948: - // return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::ICM42688: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - case SensorTypeID::BMI270: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::LSM6DS3TRC: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - case SensorTypeID::LSM6DSV: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::LSM6DSO: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - // case SensorTypeID::LSM6DSR: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - case SensorTypeID::ICM45686: - return buildSensor(BUILD_SENSOR_ARGS); - // case SensorTypeID::ICM45605: - // return buildSensor( - // BUILD_SENSOR_ARGS - // ); - default: - m_Manager->m_Logger.error( - "Unable to create sensor with type %s (%d)", - getIMUNameByType(type), - type - ); - } - return std::make_unique(sensorID); - } + ); SensorTypeID findSensorType( uint8_t sensorID, @@ -301,40 +131,7 @@ struct SensorBuilder { bool optional, PinInterface* intPin, int extraParam - ) { - sensorInterface->init(); - sensorInterface->swapIn(); - // if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuAddress)) - // { return SensorTypeID::LSM6DS3TRC; - // } - // if (SoftFusionICM42688::checkPresent(sensorID, imuAddress)) { - // return SensorTypeID::ICM42688; - //} - if (SoftFusionBMI270::checkPresent(sensorID, imuAddress)) { - return SensorTypeID::BMI270; - } - if (SoftFusionLSM6DSV::checkPresent(sensorID, imuAddress)) { - return SensorTypeID::LSM6DSV; - } - // if (SoftFusionLSM6DSO::checkPresent(sensorID, imuAddress)) { - // return SensorTypeID::LSM6DSO; - // } - // if (SoftFusionLSM6DSR::checkPresent(sensorID, imuAddress)) { - // return SensorTypeID::LSM6DSR; - // } - // if (SoftFusionMPU6050::checkPresent(sensorID, imuAddress)) { - // return SensorTypeID::MPU6050; - // } - if (SoftFusionICM45686::checkPresent(sensorID, imuAddress)) { - return SensorTypeID::ICM45686; - } - return BNO080Sensor::checkIfPresent(sensorID, imuAddress, intPin); - // if (SoftFusionICM45605::checkPresent(sensorID, imuAddress)) { - // return SensorTypeID::ICM45605; - // } - - return SensorTypeID::Unknown; - } + ); SensorTypeID findSensorType( uint8_t sensorID, @@ -344,40 +141,7 @@ struct SensorBuilder { bool optional, PinInterface* intPin, int extraParam - ) { - sensorInterface->init(); - sensorInterface->swapIn(); - // if (SoftFusionLSM6DS3TRC::checkPresent(sensorID, imuInterface)) - // { return SensorTypeID::LSM6DS3TRC; - // } - // if (SoftFusionICM42688::checkPresent(sensorID, imuInterface)) { - // return SensorTypeID::ICM42688; - //} - if (SoftFusionBMI270::checkPresent(sensorID, imuInterface)) { - return SensorTypeID::BMI270; - } - if (SoftFusionLSM6DSV::checkPresent(sensorID, imuInterface)) { - return SensorTypeID::LSM6DSV; - } - // if (SoftFusionLSM6DSO::checkPresent(sensorID, imuInterface)) { - // return SensorTypeID::LSM6DSO; - // } - // if (SoftFusionLSM6DSR::checkPresent(sensorID, imuInterface)) { - // return SensorTypeID::LSM6DSR; - // } - // if (SoftFusionMPU6050::checkPresent(sensorID, imuInterface)) { - // return SensorTypeID::MPU6050; - // } - if (SoftFusionICM45686::checkPresent(sensorID, imuInterface)) { - return SensorTypeID::ICM45686; - } - return BNO080Sensor::checkIfPresent(sensorID, sensorInterface, intPin); - // if (SoftFusionICM45605::checkPresent(sensorID, imuInterface)) { - // return SensorTypeID::ICM45605; - // } - - return SensorTypeID::Unknown; - } + ); template std::unique_ptr<::Sensor> buildSensor( @@ -490,5 +254,4 @@ struct SensorBuilder { return sensor; } }; -} // namespace Sensors -} // namespace SlimeVR +} // namespace SlimeVR::Sensors From 3138f58319c6127fe889f0e94c34721b2a199326 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 03:04:48 +0200 Subject: [PATCH 13/37] Fix copyright --- src/sensors/SensorBuilder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/SensorBuilder.h b/src/sensors/SensorBuilder.h index a874ff65f..1540e502a 100644 --- a/src/sensors/SensorBuilder.h +++ b/src/sensors/SensorBuilder.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV + 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 From 91c72d31102c82a271a8b37a237ff44bf488f88a Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 19:56:32 +0200 Subject: [PATCH 14/37] Fix some issues with glove after all the changes Add definitions for SlimeVR v1.2 Fix typo and add comments to quaternion sandwich function --- lib/math/quat.cpp | 2 +- lib/math/quat.h | 8 +++++++- src/consts.h | 7 ++++--- src/defines.h | 25 +++++++++++++++++++++---- src/sensors/SensorBuilder.cpp | 4 ++-- src/sensors/sensor.cpp | 2 +- src/sensors/sensorposition.h | 4 +++- 7 files changed, 39 insertions(+), 13 deletions(-) diff --git a/lib/math/quat.cpp b/lib/math/quat.cpp index c2d7d4680..7bf6a6114 100644 --- a/lib/math/quat.cpp +++ b/lib/math/quat.cpp @@ -230,7 +230,7 @@ void Quat::set_axis_angle(const Vector3& axis, const float& angle) { } } -void Quat::sandwitch(Vector3& v) { +void Quat::sandwich(Vector3& v) { float tempX, tempY; tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x; tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x * v.y; diff --git a/lib/math/quat.h b/lib/math/quat.h index 5383e311f..63c056a69 100644 --- a/lib/math/quat.h +++ b/lib/math/quat.h @@ -79,7 +79,13 @@ class Quat { Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq, const float& t) const; bool equalsWithEpsilon(const Quat& q2); - void sandwitch(Vector3& vector); + /** + * @brief Rotate the vector by this quaternion + * (a sandwich product) + * + * @param vector the vector to be rotated + */ + void sandwich(Vector3& vector); void set_axis_angle(const Vector3& axis, const float& angle); inline void get_axis_angle(Vector3& r_axis, double& r_angle) const { diff --git a/src/consts.h b/src/consts.h index 784851f77..9d4f9b3ad 100644 --- a/src/consts.h +++ b/src/consts.h @@ -73,15 +73,15 @@ enum class SensorTypeID : uint8_t { #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BOARD_UNKNOWN 0 -#define BOARD_SLIMEVR_LEGACY 1 -#define BOARD_SLIMEVR_DEV 2 +#define BOARD_SLIMEVR_LEGACY 1 // More ancient development version of SlimeVR +#define BOARD_SLIMEVR_DEV 2 // Ancient development version of SlimeVR #define BOARD_NODEMCU 3 #define BOARD_CUSTOM 4 #define BOARD_WROOM32 5 #define BOARD_WEMOSD1MINI 6 #define BOARD_TTGO_TBASE 7 #define BOARD_ESP01 8 -#define BOARD_SLIMEVR 9 +#define BOARD_SLIMEVR 9 // SlimeVR v1.0 & v1.1 #define BOARD_LOLIN_C3_MINI 10 #define BOARD_BEETLE32C3 11 #define BOARD_ESP32C3DEVKITM1 12 @@ -94,6 +94,7 @@ enum class SensorTypeID : uint8_t { #define BOARD_ESP32C6DEVKITC1 19 #define BOARD_GLOVE_IMU_SLIMEVR_DEV 20 // IMU Glove #define BOARD_GESTURES 21 // Used by Gestures +#define BOARD_SLIMEVR_V1_2 22 // SlimeVR v1.2 #define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BAT_EXTERNAL 1 diff --git a/src/defines.h b/src/defines.h index 42492f356..28a5d73a5 100644 --- a/src/defines.h +++ b/src/defines.h @@ -81,7 +81,7 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ #ifndef SENSOR_DESC_LIST #define MAX_SENSORS_COUNT 10 #define TRACKER_TYPE TrackerType::TRACKER_TYPE_SVR_GLOVE_LEFT -#define GLOVE_SIDE GloveSide::GLOVE_LEFT +#define GLOVE_SIDE GLOVE_LEFT #define PRIMARY_IMU_ADDRESS_ONE 0x4a #define SECONDARY_IMU_ADDRESS_TWO 0x4b @@ -177,7 +177,7 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ 0 \ ) -#if GLOVE_SIDE == GloveSide::GLOVE_LEFT +#if GLOVE_SIDE == GLOVE_LEFT #define SENSOR_INFO_LIST \ SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_LEFT_HAND) \ SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_LEFT_LITTLE_INTERMEDIATE) \ @@ -187,9 +187,9 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ SENSOR_INFO_ENTRY(5, SensorPosition::POSITION_LEFT_MIDDLE_DISTAL) \ SENSOR_INFO_ENTRY(6, SensorPosition::POSITION_LEFT_INDEX_INTERMEDIATE) \ SENSOR_INFO_ENTRY(7, SensorPosition::POSITION_LEFT_INDEX_DISTAL) \ - SENSOR_INFO_ENTRY(8, SensorPosition::SensorPosition::POSITION_LEFT_THUMB_PROXIMAL) \ + SENSOR_INFO_ENTRY(8, SensorPosition::POSITION_LEFT_THUMB_PROXIMAL) \ SENSOR_INFO_ENTRY(9, SensorPosition::POSITION_LEFT_THUMB_DISTAL) -#elif GLOVE_SDIE == GloveSide::GLOVE_RIGHT +#elif GLOVE_SDIE == GLOVE_RIGHT #define SENSOR_INFO_LIST \ SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_RIGHT_HAND) \ SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_RIGHT_LITTLE_INTERMEDIATE) \ @@ -239,6 +239,23 @@ 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_BATTERY_LEVEL 17 +#define LED_PIN 2 +#define LED_INVERTED true +#ifndef BATTERY_SHIELD_RESISTANCE +#define BATTERY_SHIELD_RESISTANCE 0 +#endif +#ifndef BATTERY_SHIELD_R1 +#define BATTERY_SHIELD_R1 10 +#endif +#ifndef BATTERY_SHIELD_R2 +#define BATTERY_SHIELD_R2 40.2 +#endif +#elif BOARD == BOARD_SLIMEVR_V1_2 #define PIN_IMU_SDA 4 #define PIN_IMU_SCL 5 #define PIN_IMU_INT 2 diff --git a/src/sensors/SensorBuilder.cpp b/src/sensors/SensorBuilder.cpp index 407768559..295696458 100644 --- a/src/sensors/SensorBuilder.cpp +++ b/src/sensors/SensorBuilder.cpp @@ -39,7 +39,7 @@ uint8_t SensorBuilder::buildAllSensors() { #define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) #define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) #define PCA_WIRE(scl, sda, addr, ch) \ - interfaceManager.pcaWireInterface().get(scl, sda, addr, ch); + interfaceManager.pcaWireInterface().get(scl, sda, addr, ch) #define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ *(new SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN)) @@ -80,7 +80,7 @@ uint8_t SensorBuilder::buildAllSensors() { SENSOR_DESC_LIST; #define SENSOR_INFO_ENTRY(ImuID, ...) \ - { m_Manager->m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); } + { m_Manager->m_Sensors[ImuID]->setSensorInfo(__VA_ARGS__); } // Apply sensor info list SENSOR_INFO_LIST; diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 4e12b2dd9..a018b5c91 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -33,7 +33,7 @@ SensorStatus Sensor::getSensorState() { void Sensor::setAcceleration(Vector3 a) { acceleration = a; - sensorOffset.sandwitch(acceleration); + sensorOffset.sandwich(acceleration); newAcceleration = true; } diff --git a/src/sensors/sensorposition.h b/src/sensors/sensorposition.h index 4503beb65..1047e82f4 100644 --- a/src/sensors/sensorposition.h +++ b/src/sensors/sensorposition.h @@ -76,4 +76,6 @@ enum class SensorPosition : uint8_t { POSITION_RIGHT_LITTLE_DISTAL = 50 }; -enum class GloveSide { NO_GLOVE = 0, GLOVE_LEFT = 1, GLOVE_RIGHT = 2 }; +#define NO_GLOVE 0 +#define GLOVE_LEFT 1 +#define GLOVE_RIGHT 2 From e08a5bfb7f5fdf1c9a5bb9805c911cee7a7bf92f Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 20:36:00 +0200 Subject: [PATCH 15/37] Add NO_WIRE definition for SPI devices --- src/defines.h | 49 +++++++++++++++++++++++++---------- src/sensors/SensorBuilder.cpp | 7 ++--- src/sensors/SensorManager.cpp | 6 ++--- src/sensors/SensorManager.h | 6 ++--- src/sensors/bno080sensor.h | 4 +-- 5 files changed, 45 insertions(+), 27 deletions(-) diff --git a/src/defines.h b/src/defines.h index 28a5d73a5..0a03baf61 100644 --- a/src/defines.h +++ b/src/defines.h @@ -28,7 +28,7 @@ // Set parameters of IMU and board used #define IMU IMU_AUTO #define SECOND_IMU IMU_AUTO -#define BOARD BOARD_SLIMEVR +#define BOARD BOARD_SLIMEVR_V1_2 #define IMU_ROTATION DEG_270 #define SECOND_IMU_ROTATION DEG_270 @@ -54,12 +54,13 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ */ #ifndef SENSOR_DESC_LIST +#if BOARD == BOARD_SLIMEVR_V1_2 #define SENSOR_DESC_LIST \ - SENSOR_DESC_ENTRY( \ + SENSOR_DESC_ENTRY( \ IMU, \ DIRECT_SPI(24'000'000, MSBFIRST, SPI_MODE3, DIRECT_PIN(15)), \ IMU_ROTATION, \ - DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \ + NO_WIRE, \ PRIMARY_IMU_OPTIONAL, \ DIRECT_PIN(PIN_IMU_INT), \ 0 \ @@ -73,8 +74,28 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ DIRECT_PIN(PIN_IMU_INT_2), \ 0 \ ) +#else +#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 \ + ) +#endif #endif - #else // Predefines for the GLOVE @@ -178,16 +199,16 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ ) #if GLOVE_SIDE == GLOVE_LEFT -#define SENSOR_INFO_LIST \ - SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_LEFT_HAND) \ - SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_LEFT_LITTLE_INTERMEDIATE) \ - SENSOR_INFO_ENTRY(2, SensorPosition::POSITION_LEFT_RING_INTERMEDIATE) \ - SENSOR_INFO_ENTRY(3, SensorPosition::POSITION_LEFT_RING_DISTAL) \ - SENSOR_INFO_ENTRY(4, SensorPosition::POSITION_LEFT_MIDDLE_INTERMEDIATE) \ - SENSOR_INFO_ENTRY(5, SensorPosition::POSITION_LEFT_MIDDLE_DISTAL) \ - SENSOR_INFO_ENTRY(6, SensorPosition::POSITION_LEFT_INDEX_INTERMEDIATE) \ - SENSOR_INFO_ENTRY(7, SensorPosition::POSITION_LEFT_INDEX_DISTAL) \ - SENSOR_INFO_ENTRY(8, SensorPosition::POSITION_LEFT_THUMB_PROXIMAL) \ +#define SENSOR_INFO_LIST \ + SENSOR_INFO_ENTRY(0, SensorPosition::POSITION_LEFT_HAND) \ + SENSOR_INFO_ENTRY(1, SensorPosition::POSITION_LEFT_LITTLE_INTERMEDIATE) \ + SENSOR_INFO_ENTRY(2, SensorPosition::POSITION_LEFT_RING_INTERMEDIATE) \ + SENSOR_INFO_ENTRY(3, SensorPosition::POSITION_LEFT_RING_DISTAL) \ + SENSOR_INFO_ENTRY(4, SensorPosition::POSITION_LEFT_MIDDLE_INTERMEDIATE) \ + SENSOR_INFO_ENTRY(5, SensorPosition::POSITION_LEFT_MIDDLE_DISTAL) \ + SENSOR_INFO_ENTRY(6, SensorPosition::POSITION_LEFT_INDEX_INTERMEDIATE) \ + SENSOR_INFO_ENTRY(7, SensorPosition::POSITION_LEFT_INDEX_DISTAL) \ + SENSOR_INFO_ENTRY(8, SensorPosition::POSITION_LEFT_THUMB_PROXIMAL) \ SENSOR_INFO_ENTRY(9, SensorPosition::POSITION_LEFT_THUMB_DISTAL) #elif GLOVE_SDIE == GLOVE_RIGHT #define SENSOR_INFO_LIST \ diff --git a/src/sensors/SensorBuilder.cpp b/src/sensors/SensorBuilder.cpp index 295696458..bc291f6b8 100644 --- a/src/sensors/SensorBuilder.cpp +++ b/src/sensors/SensorBuilder.cpp @@ -35,6 +35,7 @@ uint8_t SensorBuilder::buildAllSensors() { uint8_t activeSensorCount = 0; #define NO_PIN nullptr +#define NO_WIRE new EmptySensorInterface #define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin) #define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) #define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) @@ -256,7 +257,7 @@ SensorTypeID SensorBuilder::findSensorType( if (SoftFusionICM45686::checkPresent(sensorID, imuAddress)) { return SensorTypeID::ICM45686; } - return BNO080Sensor::checkIfPresent(sensorID, imuAddress, intPin); + return BNO080Sensor::checkPresent(sensorID, imuAddress, intPin); // if (SoftFusionICM45605::checkPresent(sensorID, imuAddress)) { // return SensorTypeID::ICM45605; // } @@ -299,12 +300,12 @@ SensorTypeID SensorBuilder::findSensorType( if (SoftFusionICM45686::checkPresent(sensorID, imuInterface)) { return SensorTypeID::ICM45686; } - return BNO080Sensor::checkIfPresent(sensorID, sensorInterface, intPin); + return BNO080Sensor::checkPresent(sensorID, sensorInterface, intPin); // if (SoftFusionICM45605::checkPresent(sensorID, imuInterface)) { // return SensorTypeID::ICM45605; // } - return SensorTypeID::Unknown; + //return SensorTypeID::Unknown; } } // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 702321af2..108e8a2ad 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -25,8 +25,7 @@ #include "SensorBuilder.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { void SensorManager::setup() { if (m_MCP.begin_I2C()) { @@ -122,5 +121,4 @@ void SensorManager::update() { #endif } -} // namespace Sensors -} // namespace SlimeVR +} // namespace Sensors::SlimeVR diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index bca457e86..3446d013d 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -38,8 +38,7 @@ #include "sensorinterface/RegisterInterface.h" #include "sensorinterface/i2cimpl.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { class SensorManager { public: @@ -68,5 +67,4 @@ class SensorManager { friend class SensorBuilder; }; -} // namespace Sensors -} // namespace SlimeVR +} // namespace Sensors::SlimeVR diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 9d4e2804a..56afb4a9b 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -64,7 +64,7 @@ class BNO080Sensor : public Sensor { bool isFlagSupported(SensorToggles toggle) const final; void sendTempIfNeeded(); - static SensorTypeID checkIfPresent( + static SensorTypeID checkPresent( uint8_t sensorID, SlimeVR::SensorInterface* sensorInterface, PinInterface* intPin @@ -78,7 +78,7 @@ class BNO080Sensor : public Sensor { } static SensorTypeID - checkIfPresent(uint8_t sensorID, uint8_t imuAddress, PinInterface* intPin) { + checkPresent(uint8_t sensorID, uint8_t imuAddress, PinInterface* intPin) { uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID; // Lazy check for if BNO is present, we only check if I2C has an address here if (I2CSCAN::hasDevOnBus(address)) { From f478dd06aef42d30453b765da7c9a3c60903d7f5 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 20:40:50 +0200 Subject: [PATCH 16/37] Fix formatting --- src/consts.h | 8 ++++---- src/sensorinterface/RegisterInterface.h | 2 +- src/sensors/ADCResistanceSensor.cpp | 6 ++---- src/sensors/ADCResistanceSensor.h | 6 ++---- src/sensors/SensorBuilder.cpp | 2 +- src/sensors/SensorManager.cpp | 2 +- src/sensors/SensorManager.h | 2 +- src/sensors/softfusion/drivers/lsm6dso.h | 17 ++++++++--------- 8 files changed, 20 insertions(+), 25 deletions(-) diff --git a/src/consts.h b/src/consts.h index 9d4f9b3ad..6961ab583 100644 --- a/src/consts.h +++ b/src/consts.h @@ -73,15 +73,15 @@ enum class SensorTypeID : uint8_t { #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BOARD_UNKNOWN 0 -#define BOARD_SLIMEVR_LEGACY 1 // More ancient development version of SlimeVR -#define BOARD_SLIMEVR_DEV 2 // Ancient development version of SlimeVR +#define BOARD_SLIMEVR_LEGACY 1 // More ancient development version of SlimeVR +#define BOARD_SLIMEVR_DEV 2 // Ancient development version of SlimeVR #define BOARD_NODEMCU 3 #define BOARD_CUSTOM 4 #define BOARD_WROOM32 5 #define BOARD_WEMOSD1MINI 6 #define BOARD_TTGO_TBASE 7 #define BOARD_ESP01 8 -#define BOARD_SLIMEVR 9 // SlimeVR v1.0 & v1.1 +#define BOARD_SLIMEVR 9 // SlimeVR v1.0 & v1.1 #define BOARD_LOLIN_C3_MINI 10 #define BOARD_BEETLE32C3 11 #define BOARD_ESP32C3DEVKITM1 12 @@ -94,7 +94,7 @@ enum class SensorTypeID : uint8_t { #define BOARD_ESP32C6DEVKITC1 19 #define BOARD_GLOVE_IMU_SLIMEVR_DEV 20 // IMU Glove #define BOARD_GESTURES 21 // Used by Gestures -#define BOARD_SLIMEVR_V1_2 22 // SlimeVR v1.2 +#define BOARD_SLIMEVR_V1_2 22 // SlimeVR v1.2 #define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BAT_EXTERNAL 1 diff --git a/src/sensorinterface/RegisterInterface.h b/src/sensorinterface/RegisterInterface.h index 91ad68627..c14aa77f8 100644 --- a/src/sensorinterface/RegisterInterface.h +++ b/src/sensorinterface/RegisterInterface.h @@ -42,4 +42,4 @@ struct RegisterInterface { virtual std::string toString() const = 0; }; -} // namespace SlimeVR::Sensors::SoftFusion +} // namespace SlimeVR::Sensors diff --git a/src/sensors/ADCResistanceSensor.cpp b/src/sensors/ADCResistanceSensor.cpp index cd92b0920..7415e8347 100644 --- a/src/sensors/ADCResistanceSensor.cpp +++ b/src/sensors/ADCResistanceSensor.cpp @@ -24,8 +24,7 @@ #include "GlobalVars.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { void ADCResistanceSensor::motionLoop() { #if ESP8266 float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution; @@ -42,5 +41,4 @@ void ADCResistanceSensor::sendData() { networkConnection.sendFlexData(sensorId, m_Data); } -} // namespace Sensors -} // namespace SlimeVR \ No newline at end of file +} // namespace SlimeVR::Sensors \ No newline at end of file diff --git a/src/sensors/ADCResistanceSensor.h b/src/sensors/ADCResistanceSensor.h index f0122324e..515671418 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -25,8 +25,7 @@ #include "sensor.h" #include "sensorinterface/SensorInterface.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { class ADCResistanceSensor : Sensor { public: static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE; @@ -69,5 +68,4 @@ class ADCResistanceSensor : Sensor { float m_Data = 0.0f; }; -} // namespace Sensors -} // namespace SlimeVR +} // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorBuilder.cpp b/src/sensors/SensorBuilder.cpp index bc291f6b8..4cc4f5232 100644 --- a/src/sensors/SensorBuilder.cpp +++ b/src/sensors/SensorBuilder.cpp @@ -305,7 +305,7 @@ SensorTypeID SensorBuilder::findSensorType( // return SensorTypeID::ICM45605; // } - //return SensorTypeID::Unknown; + // return SensorTypeID::Unknown; } } // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 108e8a2ad..651bbd307 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -121,4 +121,4 @@ void SensorManager::update() { #endif } -} // namespace Sensors::SlimeVR +} // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 3446d013d..253a2dde5 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -67,4 +67,4 @@ class SensorManager { friend class SensorBuilder; }; -} // namespace Sensors::SlimeVR +} // namespace SlimeVR::Sensors diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 56cef771c..8bab8b60c 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -128,15 +128,14 @@ struct LSM6DSO : LSM6DSOutputHandler { GyroCall&& processGyroSample, TempCall&& processTempSample ) { - LSM6DSOutputHandler:: - template bulkRead( - processAccelSample, - processGyroSample, - processTempSample, - GyrTs, - AccTs, - TempTs - ); + LSM6DSOutputHandler::template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; From 341a947419ddd0d939b3319be35647142af715d7 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 20:52:29 +0200 Subject: [PATCH 17/37] Minor fix --- src/sensors/softfusion/softfusionsensor.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index c3455cb56..cbe982667 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -38,12 +38,10 @@ namespace SlimeVR::Sensors { template < - typename T, + typename SensorType, template typename Calibrator> class SoftFusionSensor : public Sensor { - using SensorType = T; - static constexpr sensor_real_t getDefaultTempTs() { if constexpr (DirectTempReadOnly) { return DirectTempReadTs; From 8efc0986e984d747740d07d2251f700ad2558067 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 20:54:24 +0200 Subject: [PATCH 18/37] If ICM-45686 not found, ask again nicely --- src/sensors/softfusion/softfusionsensor.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index cbe982667..3efb4f594 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -453,7 +453,8 @@ class SoftFusionSensor : public Sensor { static bool checkPresent(uint8_t sensorID, uint8_t imuAddress) { uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID; - if (!I2CSCAN::hasDevOnBus(address)) { + // Ask twice, because we're nice like this + if (!I2CSCAN::hasDevOnBus(address) && !I2CSCAN::hasDevOnBus(address)) { return false; } const I2CImpl& i2c = I2CImpl(address); From 6742a0877af332d5f66c1fa7abc4527b6d88123c Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 21:25:44 +0200 Subject: [PATCH 19/37] Fix MCP interface not building --- src/sensorinterface/SensorInterfaceManager.h | 7 +++---- src/sensors/SensorBuilder.cpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/sensorinterface/SensorInterfaceManager.h b/src/sensorinterface/SensorInterfaceManager.h index 3c66f188b..5c891ff3d 100644 --- a/src/sensorinterface/SensorInterfaceManager.h +++ b/src/sensorinterface/SensorInterfaceManager.h @@ -74,10 +74,9 @@ class SensorInterfaceManager { inline auto& pcaWireInterface() { return pcaWireInterfaces; } private: - SensorInterface directPinInterfaces{[](int pin) { - return pin != 255 && pin != -1; - }}; - SensorInterface mcpPinInterfaces; + SensorInterface directPinInterfaces{ + [](int pin) { return pin != 255 && pin != -1; }}; + SensorInterface mcpPinInterfaces; SensorInterface i2cWireInterfaces; SensorInterface pcaWireInterfaces; }; diff --git a/src/sensors/SensorBuilder.cpp b/src/sensors/SensorBuilder.cpp index 4cc4f5232..149393c72 100644 --- a/src/sensors/SensorBuilder.cpp +++ b/src/sensors/SensorBuilder.cpp @@ -38,7 +38,7 @@ uint8_t SensorBuilder::buildAllSensors() { #define NO_WIRE new EmptySensorInterface #define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin) #define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) -#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) +#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(&m_Manager->m_MCP, pin) #define PCA_WIRE(scl, sda, addr, ch) \ interfaceManager.pcaWireInterface().get(scl, sda, addr, ch) #define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ From 9aa4c6cd5cd34e33f56f1d1b44e061ae55a1fcd2 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 21:28:28 +0200 Subject: [PATCH 20/37] Remove uneccessary "default" ctor from SPIImpl --- src/sensorinterface/SPIImpl.h | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/sensorinterface/SPIImpl.h b/src/sensorinterface/SPIImpl.h index 9e4f046e8..cb5d85676 100644 --- a/src/sensorinterface/SPIImpl.h +++ b/src/sensorinterface/SPIImpl.h @@ -35,13 +35,6 @@ namespace SlimeVR::Sensors { 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) From f68ce4e21cdba48b6bf0592edca2c7556982b10b Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 21:30:46 +0200 Subject: [PATCH 21/37] Remove buildSensorReal --- src/sensors/SensorBuilder.h | 65 +++++++++++++------------------------ 1 file changed, 22 insertions(+), 43 deletions(-) diff --git a/src/sensors/SensorBuilder.h b/src/sensors/SensorBuilder.h index 1540e502a..cc9fdf97a 100644 --- a/src/sensors/SensorBuilder.h +++ b/src/sensors/SensorBuilder.h @@ -152,49 +152,6 @@ struct SensorBuilder { bool optional = false, PinInterface* intPin = nullptr, int extraParam = 0 - ) { - return buildSensorReal( - sensorID, - imuInterface, - rotation, - sensorInterface, - optional, - intPin, - extraParam - ); - } - - template - std::unique_ptr<::Sensor> buildSensor( - uint8_t sensorID, - uint8_t imuAddress, - float rotation, - SensorInterface* sensorInterface, - bool optional = false, - PinInterface* intPin = nullptr, - int extraParam = 0 - ) { - uint8_t address = imuAddress > 0 ? imuAddress : ImuType::Address + sensorID; - return buildSensorReal( - sensorID, - *(new I2CImpl(address)), - rotation, - sensorInterface, - optional, - intPin, - extraParam - ); - } - - template - std::unique_ptr<::Sensor> buildSensorReal( - uint8_t sensorID, - RegisterInterface& imuInterface, - float rotation, - SensorInterface* sensorInterface, - bool optional = false, - PinInterface* intPin = nullptr, - int extraParam = 0 ) { m_Manager->m_Logger.trace( "Building IMU with: id=%d,\n\ @@ -253,5 +210,27 @@ struct SensorBuilder { sensor->motionSetup(); return sensor; } + + template + std::unique_ptr<::Sensor> buildSensor( + uint8_t sensorID, + uint8_t imuAddress, + float rotation, + SensorInterface* sensorInterface, + bool optional = false, + PinInterface* intPin = nullptr, + int extraParam = 0 + ) { + uint8_t address = imuAddress > 0 ? imuAddress : ImuType::Address + sensorID; + return buildSensor( + sensorID, + *(new I2CImpl(address)), + rotation, + sensorInterface, + optional, + intPin, + extraParam + ); + } }; } // namespace SlimeVR::Sensors From c214e38be33cac4f6383d7bf4e1a1413606a7e46 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 21:31:59 +0200 Subject: [PATCH 22/37] Invert if statement in sensorbuilder+ --- src/sensors/SensorBuilder.h | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/sensors/SensorBuilder.h b/src/sensors/SensorBuilder.h index cc9fdf97a..86c923d27 100644 --- a/src/sensors/SensorBuilder.h +++ b/src/sensors/SensorBuilder.h @@ -173,31 +173,30 @@ struct SensorBuilder { sensorInterface->init(); sensorInterface->swapIn(); - if (imuInterface.hasSensorOnBus()) { - m_Manager->m_Logger.trace( - "Sensor %d found at address %s", - sensorID + 1, - imuInterface.toString() - ); - } else { + if (!imuInterface.hasSensorOnBus()) { if (!optional) { m_Manager->m_Logger.error( "Mandatory sensor %d not found at address %s", sensorID + 1, imuInterface.toString() ); - sensor = std::make_unique(sensorID, ImuType::TypeID); + return std::make_unique(sensorID, ImuType::TypeID); } else { m_Manager->m_Logger.debug( "Optional sensor %d not found at address %s", sensorID + 1, imuInterface.toString() ); - sensor = std::make_unique(sensorID); + return std::make_unique(sensorID); } - return sensor; } + m_Manager->m_Logger.trace( + "Sensor %d found at address %s", + sensorID + 1, + imuInterface.toString() + ); + sensor = std::make_unique( sensorID, imuInterface, From 800c34e2f7c736e6fad190411e2e4b80e6373c44 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 20:40:50 +0200 Subject: [PATCH 23/37] Fix formatting --- src/consts.h | 8 ++++---- src/sensorinterface/RegisterInterface.h | 2 +- src/sensors/ADCResistanceSensor.cpp | 6 ++---- src/sensors/ADCResistanceSensor.h | 6 ++---- src/sensors/SensorBuilder.cpp | 2 +- src/sensors/SensorManager.cpp | 2 +- src/sensors/SensorManager.h | 2 +- src/sensors/softfusion/drivers/lsm6dso.h | 17 ++++++++--------- src/sensors/softfusion/softfusionsensor.h | 4 +--- 9 files changed, 21 insertions(+), 28 deletions(-) diff --git a/src/consts.h b/src/consts.h index 9d4f9b3ad..6961ab583 100644 --- a/src/consts.h +++ b/src/consts.h @@ -73,15 +73,15 @@ enum class SensorTypeID : uint8_t { #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BOARD_UNKNOWN 0 -#define BOARD_SLIMEVR_LEGACY 1 // More ancient development version of SlimeVR -#define BOARD_SLIMEVR_DEV 2 // Ancient development version of SlimeVR +#define BOARD_SLIMEVR_LEGACY 1 // More ancient development version of SlimeVR +#define BOARD_SLIMEVR_DEV 2 // Ancient development version of SlimeVR #define BOARD_NODEMCU 3 #define BOARD_CUSTOM 4 #define BOARD_WROOM32 5 #define BOARD_WEMOSD1MINI 6 #define BOARD_TTGO_TBASE 7 #define BOARD_ESP01 8 -#define BOARD_SLIMEVR 9 // SlimeVR v1.0 & v1.1 +#define BOARD_SLIMEVR 9 // SlimeVR v1.0 & v1.1 #define BOARD_LOLIN_C3_MINI 10 #define BOARD_BEETLE32C3 11 #define BOARD_ESP32C3DEVKITM1 12 @@ -94,7 +94,7 @@ enum class SensorTypeID : uint8_t { #define BOARD_ESP32C6DEVKITC1 19 #define BOARD_GLOVE_IMU_SLIMEVR_DEV 20 // IMU Glove #define BOARD_GESTURES 21 // Used by Gestures -#define BOARD_SLIMEVR_V1_2 22 // SlimeVR v1.2 +#define BOARD_SLIMEVR_V1_2 22 // SlimeVR v1.2 #define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BAT_EXTERNAL 1 diff --git a/src/sensorinterface/RegisterInterface.h b/src/sensorinterface/RegisterInterface.h index 91ad68627..c14aa77f8 100644 --- a/src/sensorinterface/RegisterInterface.h +++ b/src/sensorinterface/RegisterInterface.h @@ -42,4 +42,4 @@ struct RegisterInterface { virtual std::string toString() const = 0; }; -} // namespace SlimeVR::Sensors::SoftFusion +} // namespace SlimeVR::Sensors diff --git a/src/sensors/ADCResistanceSensor.cpp b/src/sensors/ADCResistanceSensor.cpp index cd92b0920..7415e8347 100644 --- a/src/sensors/ADCResistanceSensor.cpp +++ b/src/sensors/ADCResistanceSensor.cpp @@ -24,8 +24,7 @@ #include "GlobalVars.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { void ADCResistanceSensor::motionLoop() { #if ESP8266 float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution; @@ -42,5 +41,4 @@ void ADCResistanceSensor::sendData() { networkConnection.sendFlexData(sensorId, m_Data); } -} // namespace Sensors -} // namespace SlimeVR \ No newline at end of file +} // namespace SlimeVR::Sensors \ No newline at end of file diff --git a/src/sensors/ADCResistanceSensor.h b/src/sensors/ADCResistanceSensor.h index f0122324e..515671418 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -25,8 +25,7 @@ #include "sensor.h" #include "sensorinterface/SensorInterface.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { class ADCResistanceSensor : Sensor { public: static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE; @@ -69,5 +68,4 @@ class ADCResistanceSensor : Sensor { float m_Data = 0.0f; }; -} // namespace Sensors -} // namespace SlimeVR +} // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorBuilder.cpp b/src/sensors/SensorBuilder.cpp index bc291f6b8..4cc4f5232 100644 --- a/src/sensors/SensorBuilder.cpp +++ b/src/sensors/SensorBuilder.cpp @@ -305,7 +305,7 @@ SensorTypeID SensorBuilder::findSensorType( // return SensorTypeID::ICM45605; // } - //return SensorTypeID::Unknown; + // return SensorTypeID::Unknown; } } // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 108e8a2ad..651bbd307 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -121,4 +121,4 @@ void SensorManager::update() { #endif } -} // namespace Sensors::SlimeVR +} // namespace SlimeVR::Sensors diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 3446d013d..253a2dde5 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -67,4 +67,4 @@ class SensorManager { friend class SensorBuilder; }; -} // namespace Sensors::SlimeVR +} // namespace SlimeVR::Sensors diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 56cef771c..8bab8b60c 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -128,15 +128,14 @@ struct LSM6DSO : LSM6DSOutputHandler { GyroCall&& processGyroSample, TempCall&& processTempSample ) { - LSM6DSOutputHandler:: - template bulkRead( - processAccelSample, - processGyroSample, - processTempSample, - GyrTs, - AccTs, - TempTs - ); + LSM6DSOutputHandler::template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index c3455cb56..cbe982667 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -38,12 +38,10 @@ namespace SlimeVR::Sensors { template < - typename T, + typename SensorType, template typename Calibrator> class SoftFusionSensor : public Sensor { - using SensorType = T; - static constexpr sensor_real_t getDefaultTempTs() { if constexpr (DirectTempReadOnly) { return DirectTempReadTs; From 7d794c70a1e49eb6b1249a74f62e11e4cdc7bcc0 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 20:54:24 +0200 Subject: [PATCH 24/37] If ICM-45686 not found, ask again nicely --- src/sensors/softfusion/softfusionsensor.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index cbe982667..3efb4f594 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -453,7 +453,8 @@ class SoftFusionSensor : public Sensor { static bool checkPresent(uint8_t sensorID, uint8_t imuAddress) { uint8_t address = imuAddress > 0 ? imuAddress : Address + sensorID; - if (!I2CSCAN::hasDevOnBus(address)) { + // Ask twice, because we're nice like this + if (!I2CSCAN::hasDevOnBus(address) && !I2CSCAN::hasDevOnBus(address)) { return false; } const I2CImpl& i2c = I2CImpl(address); From 23719f032c7650c8bd9a6316a526f16211904a4b Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 21:25:44 +0200 Subject: [PATCH 25/37] Fix MCP interface not building --- src/sensorinterface/SensorInterfaceManager.h | 7 +++---- src/sensors/SensorBuilder.cpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/sensorinterface/SensorInterfaceManager.h b/src/sensorinterface/SensorInterfaceManager.h index 3c66f188b..5c891ff3d 100644 --- a/src/sensorinterface/SensorInterfaceManager.h +++ b/src/sensorinterface/SensorInterfaceManager.h @@ -74,10 +74,9 @@ class SensorInterfaceManager { inline auto& pcaWireInterface() { return pcaWireInterfaces; } private: - SensorInterface directPinInterfaces{[](int pin) { - return pin != 255 && pin != -1; - }}; - SensorInterface mcpPinInterfaces; + SensorInterface directPinInterfaces{ + [](int pin) { return pin != 255 && pin != -1; }}; + SensorInterface mcpPinInterfaces; SensorInterface i2cWireInterfaces; SensorInterface pcaWireInterfaces; }; diff --git a/src/sensors/SensorBuilder.cpp b/src/sensors/SensorBuilder.cpp index 4cc4f5232..149393c72 100644 --- a/src/sensors/SensorBuilder.cpp +++ b/src/sensors/SensorBuilder.cpp @@ -38,7 +38,7 @@ uint8_t SensorBuilder::buildAllSensors() { #define NO_WIRE new EmptySensorInterface #define DIRECT_PIN(pin) interfaceManager.directPinInterface().get(pin) #define DIRECT_WIRE(scl, sda) interfaceManager.i2cWireInterface().get(scl, sda) -#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(pin) +#define MCP_PIN(pin) interfaceManager.mcpPinInterface().get(&m_Manager->m_MCP, pin) #define PCA_WIRE(scl, sda, addr, ch) \ interfaceManager.pcaWireInterface().get(scl, sda, addr, ch) #define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ From 2ed568f4e0b752d889ba543b34bfb89cdcf8ee90 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 24 Apr 2025 22:02:46 +0200 Subject: [PATCH 26/37] Fix formatting --- src/sensorinterface/SensorInterfaceManager.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/sensorinterface/SensorInterfaceManager.h b/src/sensorinterface/SensorInterfaceManager.h index 5c891ff3d..b6eff7a4e 100644 --- a/src/sensorinterface/SensorInterfaceManager.h +++ b/src/sensorinterface/SensorInterfaceManager.h @@ -74,8 +74,9 @@ class SensorInterfaceManager { inline auto& pcaWireInterface() { return pcaWireInterfaces; } private: - SensorInterface directPinInterfaces{ - [](int pin) { return pin != 255 && pin != -1; }}; + SensorInterface directPinInterfaces{[](int pin) { + return pin != 255 && pin != -1; + }}; SensorInterface mcpPinInterfaces; SensorInterface i2cWireInterfaces; SensorInterface pcaWireInterfaces; From 14e53996ecd0c4ab5a56002447daadaa96c5bb02 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 22:08:22 +0200 Subject: [PATCH 27/37] Various cleanup --- src/sensorinterface/RegisterInterface.cpp | 45 ++++++++++++++++++++ src/sensorinterface/RegisterInterface.h | 28 +++++++++--- src/sensorinterface/SPIImpl.h | 4 +- src/sensorinterface/SensorInterfaceManager.h | 2 + src/sensors/ADCResistanceSensor.h | 4 +- src/sensors/EmptySensor.h | 15 ++++--- src/sensors/ErroneousSensor.h | 9 ++-- 7 files changed, 86 insertions(+), 21 deletions(-) create mode 100644 src/sensorinterface/RegisterInterface.cpp diff --git a/src/sensorinterface/RegisterInterface.cpp b/src/sensorinterface/RegisterInterface.cpp new file mode 100644 index 000000000..30f6a24c5 --- /dev/null +++ b/src/sensorinterface/RegisterInterface.cpp @@ -0,0 +1,45 @@ +/* SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & 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. +*/ + +#include "RegisterInterface.h" + +namespace SlimeVR::Sensors { + +[[nodiscard]] uint8_t EmptyRegisterInterface::readReg(uint8_t regAddr) const { + return 0; +}; +[[nodiscard]] uint16_t EmptyRegisterInterface::readReg16(uint8_t regAddr) const { + return 0; +}; +void EmptyRegisterInterface::writeReg(uint8_t regAddr, uint8_t value) const {}; +void EmptyRegisterInterface::writeReg16(uint8_t regAddr, uint16_t value) const {}; +void EmptyRegisterInterface::readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) + const {}; +void EmptyRegisterInterface::writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) + const {}; +[[nodiscard]] uint8_t EmptyRegisterInterface::getAddress() const { return 0; }; +bool EmptyRegisterInterface::hasSensorOnBus() { return true; }; +[[nodiscard]] std::string EmptyRegisterInterface::toString() const { return "Empty"; }; + +EmptyRegisterInterface EmptyRegisterInterface::instance; + +} // namespace SlimeVR::Sensors diff --git a/src/sensorinterface/RegisterInterface.h b/src/sensorinterface/RegisterInterface.h index c14aa77f8..cebf34a4a 100644 --- a/src/sensorinterface/RegisterInterface.h +++ b/src/sensorinterface/RegisterInterface.h @@ -1,6 +1,5 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Tailsy13 & SlimeVR Contributors +/* 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 @@ -23,6 +22,7 @@ #pragma once #include +#include #include "I2Cdev.h" @@ -31,15 +31,29 @@ namespace SlimeVR::Sensors { 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; + [[nodiscard]] virtual uint8_t readReg(uint8_t regAddr) const = 0; + [[nodiscard]] 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; + [[nodiscard]] virtual uint8_t getAddress() const = 0; virtual bool hasSensorOnBus() = 0; - virtual std::string toString() const = 0; + [[nodiscard]] virtual std::string toString() const = 0; +}; + +struct EmptyRegisterInterface : public RegisterInterface { + [[nodiscard]] uint8_t readReg(uint8_t regAddr) const final; + [[nodiscard]] uint16_t readReg16(uint8_t regAddr) const final; + void writeReg(uint8_t regAddr, uint8_t value) const final; + void writeReg16(uint8_t regAddr, uint16_t value) const final; + void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const final; + void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const final; + [[nodiscard]] uint8_t getAddress() const final; + bool hasSensorOnBus() final; + [[nodiscard]] std::string toString() const final; + + static EmptyRegisterInterface instance; }; } // namespace SlimeVR::Sensors diff --git a/src/sensorinterface/SPIImpl.h b/src/sensorinterface/SPIImpl.h index cb5d85676..1985de4e5 100644 --- a/src/sensorinterface/SPIImpl.h +++ b/src/sensorinterface/SPIImpl.h @@ -126,13 +126,13 @@ struct SPIImpl : public RegisterInterface { m_spiClass.endTransaction(); } - bool hasSensorOnBus() { + bool hasSensorOnBus() override { return true; // TODO } uint8_t getAddress() const override { return 0; } - std::string toString() const { return std::string("SPI"); } + std::string toString() const override { return std::string("SPI"); } private: SPIClass& m_spiClass; diff --git a/src/sensorinterface/SensorInterfaceManager.h b/src/sensorinterface/SensorInterfaceManager.h index 5c891ff3d..d924e9fce 100644 --- a/src/sensorinterface/SensorInterfaceManager.h +++ b/src/sensorinterface/SensorInterfaceManager.h @@ -31,6 +31,8 @@ #include "I2CPCAInterface.h" #include "I2CWireSensorInterface.h" #include "MCP23X17PinInterface.h" +#include "SensorInterface.h" +#include "i2cimpl.h" namespace SlimeVR { diff --git a/src/sensors/ADCResistanceSensor.h b/src/sensors/ADCResistanceSensor.h index 515671418..5196c52e6 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -22,8 +22,8 @@ */ #pragma once +#include "../sensorinterface/RegisterInterface.h" #include "sensor.h" -#include "sensorinterface/SensorInterface.h" namespace SlimeVR::Sensors { class ADCResistanceSensor : Sensor { @@ -41,7 +41,7 @@ class ADCResistanceSensor : Sensor { "ADCResistanceSensor", SensorTypeID::ADC_RESISTANCE, id, - *(new I2CImpl(0)), + EmptyRegisterInterface::instance, 0.0f, new SlimeVR::EmptySensorInterface ) diff --git a/src/sensors/EmptySensor.h b/src/sensors/EmptySensor.h index 65307f2b2..88cd4dacf 100644 --- a/src/sensors/EmptySensor.h +++ b/src/sensors/EmptySensor.h @@ -24,14 +24,20 @@ #ifndef SENSORS_EMPTYSENSOR_H #define SENSORS_EMPTYSENSOR_H +#include "../sensorinterface/RegisterInterface.h" #include "sensor.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { class EmptySensor : public Sensor { public: EmptySensor(uint8_t id) - : Sensor("EmptySensor", SensorTypeID::Empty, id, *(new I2CImpl(0)), 0.0){}; + : Sensor( + "EmptySensor", + SensorTypeID::Empty, + id, + EmptyRegisterInterface::instance, + 0.0 + ){}; ~EmptySensor(){}; void motionSetup() override final{}; @@ -42,7 +48,6 @@ class EmptySensor : public Sensor { return SensorStatus::SENSOR_OFFLINE; }; }; -} // namespace Sensors -} // namespace SlimeVR +} // namespace SlimeVR::Sensors #endif diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h index 0f94e2306..0b9056574 100644 --- a/src/sensors/ErroneousSensor.h +++ b/src/sensors/ErroneousSensor.h @@ -24,14 +24,14 @@ #ifndef SENSORS_ERRONEOUSSENSOR_H #define SENSORS_ERRONEOUSSENSOR_H +#include "../sensorinterface/RegisterInterface.h" #include "sensor.h" -namespace SlimeVR { -namespace Sensors { +namespace SlimeVR::Sensors { class ErroneousSensor : public Sensor { public: ErroneousSensor(uint8_t id, SensorTypeID type) - : Sensor("ErroneousSensor", type, id, *(new I2CImpl(0)), 0.0) + : Sensor("ErroneousSensor", type, id, EmptyRegisterInterface::instance, 0.0) , m_ExpectedType(type){}; ~ErroneousSensor(){}; @@ -44,7 +44,6 @@ class ErroneousSensor : public Sensor { private: SensorTypeID m_ExpectedType; }; -} // namespace Sensors -} // namespace SlimeVR +} // namespace SlimeVR::Sensors #endif From 0bba70dbe0cd61e922e88a1e1dd09e0683539e27 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 24 Apr 2025 22:15:41 +0200 Subject: [PATCH 28/37] Formattign --- src/sensorinterface/SensorInterfaceManager.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/sensorinterface/SensorInterfaceManager.h b/src/sensorinterface/SensorInterfaceManager.h index d924e9fce..06f9fb9e4 100644 --- a/src/sensorinterface/SensorInterfaceManager.h +++ b/src/sensorinterface/SensorInterfaceManager.h @@ -76,8 +76,9 @@ class SensorInterfaceManager { inline auto& pcaWireInterface() { return pcaWireInterfaces; } private: - SensorInterface directPinInterfaces{ - [](int pin) { return pin != 255 && pin != -1; }}; + SensorInterface directPinInterfaces{[](int pin) { + return pin != 255 && pin != -1; + }}; SensorInterface mcpPinInterfaces; SensorInterface i2cWireInterfaces; SensorInterface pcaWireInterfaces; From 4d963064dd01cabe906a1e5baa65d34efe036c6a Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 14 May 2025 20:33:07 +0200 Subject: [PATCH 29/37] Fix detected logic --- src/sensors/softfusion/softfusionsensor.h | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 0ec80a86e..e3ebb3d7d 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -60,14 +60,8 @@ class SoftFusionSensor : public Sensor { bool detected() const { 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", - SensorType::Regs::WhoAmI::reg, - SensorType::Regs::WhoAmI::value, - value - ); - return false; + if (SensorType::Regs::WhoAmI::value == value) { + return true; } m_Logger.error( From 9fb5b102c38e18ac18dc6376415940b7dd940e36 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 14 May 2025 20:34:42 +0200 Subject: [PATCH 30/37] Remove unnecessary Self using --- src/sensors/softfusion/softfusionsensor.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index e3ebb3d7d..daf49f3d6 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -46,8 +46,6 @@ namespace SlimeVR::Sensors { template typename Calibrator> class SoftFusionSensor : public Sensor { - using Self = SoftFusionSensor; - using Consts = IMUConsts; using RawSensorT = typename Consts::RawSensorT; From 1fe33016b9c74c4b67c6451089620ec471f907b5 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 14 May 2025 21:41:43 +0200 Subject: [PATCH 31/37] For some reason this fixes memory corruption issues --- src/sensors/softfusion/drivers/bmi270fw.h | 7 +++---- src/sensors/softfusion/drivers/icm45base.h | 10 +++++----- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/sensors/softfusion/drivers/bmi270fw.h b/src/sensors/softfusion/drivers/bmi270fw.h index 08479c8e4..7c16a4e25 100644 --- a/src/sensors/softfusion/drivers/bmi270fw.h +++ b/src/sensors/softfusion/drivers/bmi270fw.h @@ -39,7 +39,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { -const uint8_t bmi270_firmware[] = { +const uint8_t PROGMEM bmi270_firmware[] = { 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc, 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0x21, 0x2e, @@ -625,7 +625,6 @@ const uint8_t bmi270_firmware[] = { 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1 -}; + 0x00, 0xc1}; -} \ No newline at end of file +} diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index cfa05e30c..2dc43e668 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -153,12 +153,12 @@ struct ICM45Base { return true; } - void bulkRead(DriverCallbacks&& callbacks) { - // Allocate statically so that it does not take up stack space, which - // can result in stack overflow and panic - constexpr size_t MaxReadings = 8; - static std::array read_buffer; + // Allocate statically so that it does not take up stack space, which + // can result in stack overflow and panic + static constexpr size_t MaxReadings = 8; + std::array read_buffer; + void bulkRead(DriverCallbacks&& callbacks) { constexpr int16_t InvalidReading = -32768; size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount); From 4b126b347f9af3c610d4273802e9e89f72d9b095 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Wed, 14 May 2025 21:49:19 +0200 Subject: [PATCH 32/37] Formatting --- src/sensors/softfusion/drivers/bmi270fw.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sensors/softfusion/drivers/bmi270fw.h b/src/sensors/softfusion/drivers/bmi270fw.h index 7c16a4e25..02c3b5ee7 100644 --- a/src/sensors/softfusion/drivers/bmi270fw.h +++ b/src/sensors/softfusion/drivers/bmi270fw.h @@ -625,6 +625,7 @@ const uint8_t PROGMEM bmi270_firmware[] = { 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1}; + 0x00, 0xc1 +}; } From 81f4fcfce9c44abf74045104643c3967262f2e75 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 17 May 2025 10:32:40 +0200 Subject: [PATCH 33/37] This actually works this time --- src/sensors/softfusion/drivers/icm45base.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 2dc43e668..182e4a48a 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -148,17 +148,21 @@ struct ICM45Base { BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::value ); + + read_buffer.resize(FullFifoEntrySize * MaxReadings); + delay(1); return true; } - // Allocate statically so that it does not take up stack space, which - // can result in stack overflow and panic static constexpr size_t MaxReadings = 8; - std::array read_buffer; + std::vector read_buffer; void bulkRead(DriverCallbacks&& callbacks) { + // Allocate statically so that it does not take up stack space, which + // can result in stack overflow and panic + constexpr int16_t InvalidReading = -32768; size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount); From 1421d6d1ede22938de2dfe42b583408e94b29002 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 17 May 2025 10:36:39 +0200 Subject: [PATCH 34/37] Small cleanup --- src/sensors/softfusion/drivers/icm45base.h | 45 ++++++++++------------ 1 file changed, 20 insertions(+), 25 deletions(-) diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 182e4a48a..5646fe355 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -157,41 +157,36 @@ struct ICM45Base { } static constexpr size_t MaxReadings = 8; + // Allocate on heap so that it does not take up stack space, which can result in + // stack overflow and panic std::vector read_buffer; void bulkRead(DriverCallbacks&& callbacks) { - // Allocate statically so that it does not take up stack space, which - // can result in stack overflow and panic - constexpr int16_t InvalidReading = -32768; size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount); - if (fifo_packets >= 1) { - // - // AN-000364 - // 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA - // - // Description: When in FIFO streaming mode, a FIFO empty event - // (caused by host reading the last byte of the last FIFO frame) can - // cause FIFO data corruption in the first FIFO frame that arrives - // after the FIFO empty condition. Once the issue is triggered, the - // FIFO state is compromised and cannot recover. FIFO must be set in - // bypass mode to flush out the wrong state - // - // When operating in FIFO streaming mode, if FIFO threshold - // interrupt is triggered with M number of FIFO frames accumulated - // in the FIFO buffer, the host should only read the first M-1 - // number of FIFO frames. This prevents the FIFO empty event, that - // can cause FIFO data corruption, from happening. - // - --fifo_packets; - } - - if (fifo_packets == 0) { + if (fifo_packets <= 1) { return; } + // AN-000364 + // 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA + // + // Description: When in FIFO streaming mode, a FIFO empty event + // (caused by host reading the last byte of the last FIFO frame) can + // cause FIFO data corruption in the first FIFO frame that arrives + // after the FIFO empty condition. Once the issue is triggered, the + // FIFO state is compromised and cannot recover. FIFO must be set in + // bypass mode to flush out the wrong state + // + // When operating in FIFO streaming mode, if FIFO threshold + // interrupt is triggered with M number of FIFO frames accumulated + // in the FIFO buffer, the host should only read the first M-1 + // number of FIFO frames. This prevents the FIFO empty event, that + // can cause FIFO data corruption, from happening. + --fifo_packets; + fifo_packets = std::min(fifo_packets, MaxReadings); size_t bytes_to_read = fifo_packets * FullFifoEntrySize; From 3975bc202a4d0f2cc77c6458d86e37c616594974 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Mon, 2 Jun 2025 21:25:18 +0200 Subject: [PATCH 35/37] Remove some unused includes --- src/sensors/softfusion/softfusionsensor.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 0dd8287a5..14c972c90 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -26,18 +26,13 @@ #include #include -#include #include -#include -#include #include "../../GlobalVars.h" #include "../../sensorinterface/SensorInterface.h" -#include "../../sensorinterface/i2cimpl.h" #include "../RestCalibrationDetector.h" #include "../sensor.h" #include "TempGradientCalculator.h" -#include "drivers/callbacks.h" #include "imuconsts.h" #include "motionprocessing/types.h" #include "sensors/SensorFusion.h" From 1c31f8e914dbdba15ca5d221721c2a3d23e63a7e Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Mon, 2 Jun 2025 21:25:54 +0200 Subject: [PATCH 36/37] Formatting --- src/sensors/softfusion/CalibrationBase.h | 2 +- src/sensors/softfusion/SoftfusionCalibration.h | 3 +-- .../runtimecalibration/RuntimeCalibration.h | 3 +-- src/sensors/softfusion/softfusionsensor.h | 17 ++++++++--------- 4 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/sensors/softfusion/CalibrationBase.h b/src/sensors/softfusion/CalibrationBase.h index 16d823f46..d132a365b 100644 --- a/src/sensors/softfusion/CalibrationBase.h +++ b/src/sensors/softfusion/CalibrationBase.h @@ -64,7 +64,7 @@ class CalibrationBase { } virtual void checkStartupCalibration() {} - virtual void startCalibration(int calibrationType) {}; + virtual void startCalibration(int calibrationType){}; virtual bool calibrationMatches( const SlimeVR::Configuration::SensorConfig& sensorCalibration diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h index 33868544d..b4b7850b2 100644 --- a/src/sensors/softfusion/SoftfusionCalibration.h +++ b/src/sensors/softfusion/SoftfusionCalibration.h @@ -166,8 +166,7 @@ class SoftfusionCalibrator : public CalibrationBase { saveCalibration(); } - bool calibrationMatches( - const Configuration::SensorConfig& sensorCalibration + bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration ) final { return sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::SFUSION diff --git a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h index 26e0f85b4..b6567e6bc 100644 --- a/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h +++ b/src/sensors/softfusion/runtimecalibration/RuntimeCalibration.h @@ -64,8 +64,7 @@ class RuntimeCalibrator : public Sensors::CalibrationBase { activeCalibration.T_Ts = Consts::getDefaultTempTs(); } - bool calibrationMatches( - const Configuration::SensorConfig& sensorCalibration + bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration ) final { return sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::RUNTIME_CALIBRATION diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 14c972c90..96b19b8de 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -162,13 +162,13 @@ class SoftFusionSensor : public Sensor { uint8_t = 0 ) : Sensor( - SensorType::Name, - SensorType::Type, - id, - registerInterface, - rotation, - sensorInterface - ) + SensorType::Name, + SensorType::Type, + id, + registerInterface, + rotation, + sensorInterface + ) , m_fusion( SensorType::SensorVQFParams, SensorType::GyrTs, @@ -286,8 +286,7 @@ class SoftFusionSensor : public Sensor { // zero-ed out if (calibrator.calibrationMatches(sensorCalibration)) { calibrator.assignCalibration(sensorCalibration); - } else if (sensorCalibration.type - == SlimeVR::Configuration::SensorConfigType::NONE) { + } else if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::NONE) { m_Logger.warn( "No calibration data found for sensor %d, ignoring...", sensorId From adba3fd9c01953c5df513e184ebb77d82b82eb33 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 12 Jun 2025 04:20:39 +0200 Subject: [PATCH 37/37] Mag support (Attempt 2) (#458) * WhoAmI check working * In theory this should be setting up the mag * Not sure how that happened * Add magnetometer status to GET TEST and GET INFO * Formatting * Formatting 2 --- src/sensors/SensorToggles.cpp | 24 +++ src/sensors/SensorToggles.h | 9 + src/sensors/bno080sensor.cpp | 7 + src/sensors/sensor.cpp | 12 +- src/sensors/sensor.h | 5 + src/sensors/softfusion/drivers/icm45base.h | 201 +++++++++++++++++++++ src/sensors/softfusion/imuconsts.h | 9 + src/sensors/softfusion/magdriver.cpp | 132 ++++++++++++++ src/sensors/softfusion/magdriver.h | 77 ++++++++ src/sensors/softfusion/softfusionsensor.h | 38 ++++ src/serial/serialcommands.cpp | 10 + 11 files changed, 521 insertions(+), 3 deletions(-) create mode 100644 src/sensors/softfusion/magdriver.cpp create mode 100644 src/sensors/softfusion/magdriver.h diff --git a/src/sensors/SensorToggles.cpp b/src/sensors/SensorToggles.cpp index e999dddd2..fe59e7e54 100644 --- a/src/sensors/SensorToggles.cpp +++ b/src/sensors/SensorToggles.cpp @@ -25,3 +25,27 @@ bool SensorToggleState::getToggle(SensorToggles toggle) const { } return false; } + +void SensorToggleState::onToggleChange( + std::function&& 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"; +} diff --git a/src/sensors/SensorToggles.h b/src/sensors/SensorToggles.h index 950adc040..d68414422 100644 --- a/src/sensors/SensorToggles.h +++ b/src/sensors/SensorToggles.h @@ -24,6 +24,8 @@ #pragma once #include +#include +#include #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&& callback); + + static const char* toggleToString(SensorToggles toggle); + private: + std::optional> callback; + void emitToggleChange(SensorToggles toggle, bool state) const; + bool magEnabled = !USE_6_AXIS; bool calibrationEnabled = true; bool tempGradientCalibrationEnabled = true; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index b34292c96..d5c1044f5 100644 --- a/src/sensors/bno080sensor.cpp +++ b/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() { diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 2a9058d03..ea484d24c 100644 --- a/src/sensors/sensor.cpp +++ b/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(); } diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 616bef986..ee1417654 100644 --- a/src/sensors/sensor.h +++ b/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; }; diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 5646fe355..de08e4f17 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/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 + uint8_t readBankRegister() { + uint8_t buffer; + readBankRegister(&buffer, sizeof(buffer)); + return buffer; + } + + template + void readBankRegister(T* buffer, size_t length) { + uint8_t data[] = { + static_cast(Reg::bank), + Reg::reg, + }; + + auto* bufferBytes = reinterpret_cast(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 + void writeBankRegister() { + writeBankRegister(&Reg::value, sizeof(Reg::value)); + } + + template + void writeBankRegister(T* buffer, size_t length) { + auto* bufferBytes = reinterpret_cast(buffer); + + uint8_t data[] = { + static_cast(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 + void writeBankRegister(uint8_t value) { + writeBankRegister(&value, sizeof(value)); + } + + void setAuxId(uint8_t deviceId) { + writeBankRegister(deviceId); + } + + uint8_t readAux(uint8_t address) { + writeBankRegister(address); + + writeBankRegister( + (0b1 << 7) // Last transaction + | (0b0 << 6) // Channel 0 + | (0b01 << 4) // Read with register + | (0b0001 << 0) // Read 1 byte + ); + writeBankRegister( + (0b0 << 6) // No restarts + | (0b0 << 3) // Fast mode + | (0b1 << 0) // Start transaction + ); + + uint8_t lastStatus; + while ((lastStatus = readBankRegister()) + & 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(); + } + + void writeAux(uint8_t address, uint8_t value) { + writeBankRegister(address); + writeBankRegister(value); + writeBankRegister( + (0b1 << 7) // Last transaction + | (0b0 << 6) // Channel 0 + | (0b01 << 4) // Read with register + | (0b0001 << 0) // Read 1 byte + ); + writeBankRegister( + (0b0 << 6) // No restarts + | (0b0 << 3) // Fast mode + | (0b1 << 0) // Start transaction + ); + + uint8_t lastStatus; + while ((lastStatus = readBankRegister()) + & 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 diff --git a/src/sensors/softfusion/imuconsts.h b/src/sensors/softfusion/imuconsts.h index 3ef880045..9ee0eae1e 100644 --- a/src/sensors/softfusion/imuconsts.h +++ b/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; + } + }(); }; diff --git a/src/sensors/softfusion/magdriver.cpp b/src/sensors/softfusion/magdriver.cpp new file mode 100644 index 000000000..8a602bdaf --- /dev/null +++ b/src/sensors/softfusion/magdriver.cpp @@ -0,0 +1,132 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & 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. +*/ + +#include "magdriver.h" + +namespace SlimeVR::Sensors::SoftFusion { + +std::vector MagDriver::supportedMags{ + MagDefinition{ + .name = "QMC6309", + + .deviceId = 0x7c, + + .whoAmIReg = 0x00, + .expectedWhoAmI = 0x90, + + .dataWidth = MagDataWidth::SixByte, + .dataReg = 0x01, + + .setup = + [](MagInterface& interface) { + interface.writeByte(0x0b, 0x80); + interface.writeByte(0x0b, 0x00); // Soft reset + delay(10); + interface.writeByte(0x0b, 0x48); // Set/reset on, 8g full range, 200Hz + interface.writeByte( + 0x0a, + 0x21 + ); // LP filter 2, 8x Oversampling, normal mode + return true; + }, + }, + MagDefinition{ + .name = "IST8306", + + .deviceId = 0x19, + + .whoAmIReg = 0x00, + .expectedWhoAmI = 0x06, + + .dataWidth = MagDataWidth::SixByte, + .dataReg = 0x11, + + .setup = + [](MagInterface& interface) { + interface.writeByte(0x32, 0x01); // Soft reset + delay(50); + interface.writeByte(0x30, 0x20); // Noise suppression: low + interface.writeByte(0x41, 0x2d); // Oversampling: 32X + interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz + return true; + }, + }, +}; + +bool MagDriver::init(MagInterface&& interface, bool supports9ByteMags) { + for (auto& mag : supportedMags) { + interface.setDeviceId(mag.deviceId); + + logger.info("Trying mag %s!", mag.name); + + uint8_t whoAmI = interface.readByte(mag.whoAmIReg); + if (whoAmI != mag.expectedWhoAmI) { + continue; + } + + if (!supports9ByteMags && mag.dataWidth == MagDataWidth::NineByte) { + logger.error("The sensor doesn't support this mag!"); + return false; + } + + logger.info("Found mag %s! Initializing", mag.name); + + if (!mag.setup(interface)) { + logger.error("Mag %s failed to initialize!", mag.name); + return false; + } + + detectedMag = mag; + + break; + } + + this->interface = interface; + return detectedMag.has_value(); +} + +void MagDriver::startPolling() const { + if (!detectedMag) { + return; + } + + interface.startPolling(detectedMag->dataReg, detectedMag->dataWidth); +} + +void MagDriver::stopPolling() const { + if (!detectedMag) { + return; + } + + interface.stopPolling(); +} + +const char* MagDriver::getAttachedMagName() const { + if (!detectedMag) { + return nullptr; + } + + return detectedMag->name; +} + +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/magdriver.h b/src/sensors/softfusion/magdriver.h new file mode 100644 index 000000000..92fe94c34 --- /dev/null +++ b/src/sensors/softfusion/magdriver.h @@ -0,0 +1,77 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & 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 "logging/Logger.h" +#include "sensorinterface/RegisterInterface.h" + +namespace SlimeVR::Sensors::SoftFusion { + +enum class MagDataWidth { + SixByte, + NineByte, +}; + +struct MagInterface { + std::function readByte; + std::function writeByte; + std::function setDeviceId; + std::function startPolling; + std::function stopPolling; +}; + +struct MagDefinition { + const char* name; + + uint8_t deviceId; + + uint8_t whoAmIReg; + uint8_t expectedWhoAmI; + + MagDataWidth dataWidth; + uint8_t dataReg; + + std::function setup; +}; + +class MagDriver { +public: + bool init(MagInterface&& interface, bool supports9ByteMags); + void startPolling() const; + void stopPolling() const; + [[nodiscard]] const char* getAttachedMagName() const; + +private: + std::optional detectedMag; + MagInterface interface; + + static std::vector supportedMags; + + Logging::Logger logger{"MagDriver"}; +}; + +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 96b19b8de..49d37c3cf 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -36,6 +36,7 @@ #include "imuconsts.h" #include "motionprocessing/types.h" #include "sensors/SensorFusion.h" +#include "sensors/softfusion/magdriver.h" namespace SlimeVR::Sensors { @@ -326,6 +327,37 @@ class SoftFusionSensor : public Sensor { working = true; calibrator.checkStartupCalibration(); + + if constexpr (Consts::SupportsMags) { + magDriver.init( + SoftFusion::MagInterface{ + .readByte + = [&](uint8_t address) { return m_sensor.readAux(address); }, + .writeByte = [&](uint8_t address, uint8_t value) {}, + .setDeviceId + = [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); }, + .startPolling + = [&](uint8_t dataReg, SoftFusion::MagDataWidth dataWidth + ) { m_sensor.startAuxPolling(dataReg, dataWidth); }, + .stopPolling = [&]() { m_sensor.stopAuxPolling(); }, + }, + Consts::Supports9ByteMag + ); + + if (toggles.getToggle(SensorToggles::MagEnabled)) { + magDriver.startPolling(); + } + } + + toggles.onToggleChange([&](SensorToggles toggle, bool value) { + if (toggle == SensorToggles::MagEnabled) { + if (value) { + magDriver.startPolling(); + } else { + magDriver.stopPolling(); + } + } + }); } void startCalibration(int calibrationType) final { @@ -351,6 +383,8 @@ class SoftFusionSensor : public Sensor { RestCalibrationDetector calibrationDetector; + SoftFusion::MagDriver magDriver; + static bool checkPresent(const RegisterInterface& imuInterface) { I2Cdev::readTimeout = 100; auto value = imuInterface.readReg(SensorType::Regs::WhoAmI::reg); @@ -369,6 +403,10 @@ class SoftFusionSensor : public Sensor { return false; } } + + const char* getAttachedMagnetometer() const final { + return magDriver.getAttachedMagName(); + } }; } // namespace SlimeVR::Sensors diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 57dbdd626..00ce408bb 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -164,6 +164,10 @@ void printState() { sensor->isWorking() ? "true" : "false", sensor->getHadData() ? "true" : "false" ); + const char* mag = sensor->getAttachedMagnetometer(); + if (mag) { + logger.info("Sensor[%d] magnetometer: %s", sensor->getSensorId(), mag); + } } logger.info( "Battery voltage: %.3f, level: %.1f%%", @@ -288,6 +292,12 @@ void cmdGet(CmdParser* parser) { sensor0->isWorking() ? "true" : "false", sensor0->getHadData() ? "true" : "false" ); + + const char* mag = sensor0->getAttachedMagnetometer(); + if (mag) { + logger.info("[TEST] Sensor[0] magnetometer: %s", mag); + } + if (!sensor0->getHadData()) { logger.error("[TEST] Sensor[0] didn't send any data yet!"); } else {