diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index eb31a99c8..488f8d790 100644 --- a/lib/bno080/BNO080.h +++ b/lib/bno080/BNO080.h @@ -50,7 +50,7 @@ #include #include #include -#include "PinInterface.h" +#include //The default I2C address for the BNO080 on the SparkX breakout is 0x4B. 0x4A is also possible. #define BNO080_DEFAULT_ADDRESS 0x4B diff --git a/lib/bno080/PinInterface.h b/lib/bno080/PinInterface.h index 1baac35fd..e72d97403 100644 --- a/lib/bno080/PinInterface.h +++ b/lib/bno080/PinInterface.h @@ -1,34 +1,34 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Eiren Rain & SlimeVR contributors + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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: + 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 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. + 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 -class PinInterface -{ +class PinInterface { public: virtual bool init() { return true; }; virtual int digitalRead() = 0; virtual void pinMode(uint8_t mode) = 0; virtual void digitalWrite(uint8_t val) = 0; + virtual float analogRead() = 0; }; diff --git a/src/batterymonitor.h b/src/batterymonitor.h index e6a38b06e..87661800f 100644 --- a/src/batterymonitor.h +++ b/src/batterymonitor.h @@ -27,20 +27,10 @@ #include #include +#include "consts.h" #include "globals.h" #include "logging/Logger.h" -#if ESP8266 -#define ADCResolution 1023.0 // ESP8266 has 10bit ADC -#define ADCVoltageMax 1.0 // ESP8266 input is 1.0 V = 1023.0 -#endif -#ifndef ADCResolution -#define ADCResolution 1023.0 -#endif -#ifndef ADCVoltageMax -#define ADCVoltageMax 1.0 -#endif - #ifndef BATTERY_SHIELD_RESISTANCE #define BATTERY_SHIELD_RESISTANCE 180.0 #endif diff --git a/src/consts.h b/src/consts.h index 74698f620..17f7f94a2 100644 --- a/src/consts.h +++ b/src/consts.h @@ -68,6 +68,7 @@ enum class SensorTypeID : uint8_t { #define IMU_MPU6050_SF SoftFusionMPU6050 #define IMU_ICM45686 SoftFusionICM45686 #define IMU_ICM45605 SoftFusionICM45605 +#define SENSOR_ADC ADCResistanceSensor #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware @@ -173,6 +174,21 @@ enum class TrackerType : uint8_t { #define CURRENT_CONFIGURATION_VERSION 1 +#if ESP8266 +#define ADCResolution 1023.0 // ESP8266 has 10bit ADC +#define ADCVoltageMax 1.0 // ESP8266 input is 1.0 V = 1023.0 +#elif ESP32 +#define ADCResolution 4095.0 // ESP32 has 12bit ADC +#define ADCVoltageMax 2.5 // ESP32 input is 2.5 V = 4095.0 by default +#endif + +#ifndef ADCResolution +#define ADCResolution 1023.0 +#endif +#ifndef ADCVoltageMax +#define ADCVoltageMax 1.0 +#endif + #include "sensors/sensorposition.h" #endif // SLIMEVR_CONSTS_H_ diff --git a/src/sensorinterface/ADS111xInterface.cpp b/src/sensorinterface/ADS111xInterface.cpp new file mode 100644 index 000000000..d0ad763c7 --- /dev/null +++ b/src/sensorinterface/ADS111xInterface.cpp @@ -0,0 +1,131 @@ +/* + 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 "ADS111xInterface.h" + +#include + +namespace SlimeVR { + +ADS111xInterface::ADS111xInterface( + SensorInterface* interface, + PinInterface* drdy, + uint8_t address +) + : interface{interface} + , drdy{drdy} + , address{address} {} + +bool ADS111xInterface::init() { + Registers::Config config{ + .compQue = 0b11, // Disable comparator + .compLat = 0b0, // Doesn't matter + .compPol = 0b0, // Doesn't matter + .compMode = 0b0, // Doesn't matter + .dr = 0b100, // Doesn't matter + .mode = 0b1, // Single-shot + .pga = 0b010, // Gain (FSR): 2.048V + .mux = 0b000, // Doesn't matter + .os = 0b0, // Don't start read + }; + + if (!writeRegister(Registers::Addresses::Config, config)) { + logger.error("Couldn't initialize ADS interface!\n"); + return false; + } + + // Enable conversion ready functionality + writeRegister(Registers::Addresses::HiThresh, static_cast(0x8000)); + writeRegister(Registers::Addresses::LoThresh, static_cast(0x0000)); + + drdy->pinMode(INPUT); + + return true; +} + +float ADS111xInterface::read(uint8_t channel) { + if (__builtin_popcount(usedPins) == 1) { + // Just read the last sample + uint16_t value = readRegister(Registers::Addresses::Conversion); + + return static_cast(value) / maxValue; + } + + Registers::Config config{ + .compQue = 0b00, // Alert after every sample + .compLat = 0b1, // Latch alert + .compPol = 0b0, // Doesn't matter + .compMode = 0b0, // Doesn't matter + .dr = 0b111, // 860 samples per second + .mode = 0b1, // Single-shot + .pga = 0b010, // Gain (FSR): 2.048V + .mux = static_cast(0b100 | channel), // Current channel + .os = 0b1, // Start read + }; + + writeRegister(Registers::Addresses::Config, config); + + while (drdy->digitalRead()) + ; + + uint16_t value = readRegister(Registers::Addresses::Conversion); + + return static_cast(value) / maxValue; +} + +uint16_t ADS111xInterface::readRegister(Registers::Addresses reg) { + Wire.beginTransmission(address); + Wire.write(static_cast(Registers::Addresses::Conversion)); + Wire.endTransmission(); + + Wire.beginTransmission(address); + Wire.requestFrom(address, 2); + uint8_t msb = Wire.read(); + uint8_t lsb = Wire.read(); + Wire.endTransmission(); + + return (msb << 8) | lsb; +} + +void ADS111xInterface::registerChannel(uint8_t channel) { + usedPins |= 1 << channel; + if (__builtin_popcount(usedPins) != 1) { + return; + } + + // If we have only one channel used, just set up continuous reads + Registers::Config config{ + .compQue = 0b11, // Disable comparator + .compLat = 0b0, // Doesn't matter + .compPol = 0b0, // Doesn't matter + .compMode = 0b0, // Doesn't matter + .dr = 0b100, // 128 samples per second + .mode = 0b0, // Continuous mode + .pga = 0b010, // Gain (FSR): 2.048V + .mux = static_cast(0b100 | channel), // Use the channel + .os = 0b1, // Start reads + }; + + writeRegister(Registers::Addresses::Config, config); +} + +} // namespace SlimeVR diff --git a/src/sensorinterface/ADS111xInterface.h b/src/sensorinterface/ADS111xInterface.h new file mode 100644 index 000000000..931051e9b --- /dev/null +++ b/src/sensorinterface/ADS111xInterface.h @@ -0,0 +1,97 @@ +/* + 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 "DirectPinInterface.h" +#include "I2CWireSensorInterface.h" +#include "SensorInterface.h" + +namespace SlimeVR { + +class ADS111xInterface { +public: + ADS111xInterface(SensorInterface* interface, PinInterface* drdy, uint8_t address); + bool init(); + + float read(uint8_t channel); + +private: + static constexpr uint32_t maxValue = 0x7fff; + + struct Registers { + enum class Addresses : uint8_t { + Conversion = 0b00, + Config = 0b01, + LoThresh = 0b10, + HiThresh = 0b11, + }; + struct Config { + uint8_t compQue : 2; + uint8_t compLat : 1; + uint8_t compPol : 1; + uint8_t compMode : 1; + uint8_t dr : 3; + uint8_t mode : 1; + uint8_t pga : 3; + uint8_t mux : 3; + uint8_t os : 1; + }; + }; + static_assert(sizeof(Registers::Config) == 2); + + template + bool writeRegister(Registers::Addresses reg, T value) { + static_assert(sizeof(T) == 2); + + interface->swapIn(); + Wire.beginTransmission(address); + Wire.write(static_cast(reg)); + auto* bytes = reinterpret_cast(&value); + Wire.write(bytes[1]); + Wire.write(bytes[0]); + auto result = Wire.endTransmission(); + return result == 0; + } + + void registerChannel(uint8_t channel); + + uint16_t readRegister(Registers::Addresses red); + + SensorInterface* interface; + PinInterface* drdy; + uint8_t address; + uint8_t counter = 0; + uint8_t usedPins = 0x0; + + Logging::Logger logger = Logging::Logger("ADS111x"); + + friend class ADS111xPin; +}; + +} // namespace SlimeVR diff --git a/src/sensorinterface/ADS111xPin.cpp b/src/sensorinterface/ADS111xPin.cpp new file mode 100644 index 000000000..cc07b549c --- /dev/null +++ b/src/sensorinterface/ADS111xPin.cpp @@ -0,0 +1,43 @@ +/* + 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 "ADS111xPin.h" + +#include + +namespace SlimeVR { + +ADS111xPin::ADS111xPin(ADS111xInterface* interface, uint8_t channel) + : ads111x{interface} + , channel{channel} { + assert(channel < 4); + + interface->registerChannel(channel); +} + +int ADS111xPin::digitalRead() { return analogRead() >= 0.5f; } +void ADS111xPin::pinMode(uint8_t mode) {} +void ADS111xPin::digitalWrite(uint8_t val) {} + +float ADS111xPin::analogRead() { return ads111x->read(channel); } + +}; // namespace SlimeVR diff --git a/src/sensorinterface/ADS111xPin.h b/src/sensorinterface/ADS111xPin.h new file mode 100644 index 000000000..be38485de --- /dev/null +++ b/src/sensorinterface/ADS111xPin.h @@ -0,0 +1,46 @@ +/* + 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 + +#include "ADS111xInterface.h" + +#pragma once + +namespace SlimeVR { + +class ADS111xPin : public PinInterface { +public: + ADS111xPin(ADS111xInterface* interface, uint8_t channel); + + int digitalRead() override final; + void pinMode(uint8_t mode) override final; + void digitalWrite(uint8_t val); + float analogRead(); + +private: + ADS111xInterface* ads111x; + uint8_t channel; +}; + +}; // namespace SlimeVR diff --git a/src/sensorinterface/DirectPinInterface.cpp b/src/sensorinterface/DirectPinInterface.cpp index dc89c4165..5d7033691 100644 --- a/src/sensorinterface/DirectPinInterface.cpp +++ b/src/sensorinterface/DirectPinInterface.cpp @@ -22,8 +22,18 @@ */ #include "DirectPinInterface.h" +#include "../consts.h" + int DirectPinInterface::digitalRead() { return ::digitalRead(_pinNum); } void DirectPinInterface::pinMode(uint8_t mode) { ::pinMode(_pinNum, mode); } -void DirectPinInterface::digitalWrite(uint8_t val) { ::digitalWrite(_pinNum, val); } \ No newline at end of file +void DirectPinInterface::digitalWrite(uint8_t val) { ::digitalWrite(_pinNum, val); } + +float DirectPinInterface::analogRead() { +#if ESP8266 + return static_cast(::analogRead(_pinNum)) / ADCResolution; +#elif ESP32 + return static_cast(::analogReadMilliVolts(_pinNum)) / 1000 / ADCVoltageMax; +#endif +} diff --git a/src/sensorinterface/DirectPinInterface.h b/src/sensorinterface/DirectPinInterface.h index 56ae9ed80..b778fa34b 100644 --- a/src/sensorinterface/DirectPinInterface.h +++ b/src/sensorinterface/DirectPinInterface.h @@ -24,7 +24,8 @@ #define _H_DIRECT_PIN_INTERFACE_ #include -#include + +#include "PinInterface.h" /** * Pin interface using direct pins @@ -32,12 +33,13 @@ */ class DirectPinInterface : public PinInterface { public: - DirectPinInterface(uint8_t pin) + explicit DirectPinInterface(uint8_t pin) : _pinNum(pin){}; int digitalRead() override final; void pinMode(uint8_t mode) override final; void digitalWrite(uint8_t val) override final; + float analogRead() override final; private: uint8_t _pinNum; diff --git a/src/sensorinterface/MCP23X17PinInterface.cpp b/src/sensorinterface/MCP23X17PinInterface.cpp index 68eb9f544..bc59a42f2 100644 --- a/src/sensorinterface/MCP23X17PinInterface.cpp +++ b/src/sensorinterface/MCP23X17PinInterface.cpp @@ -28,4 +28,6 @@ void MCP23X17PinInterface::pinMode(uint8_t mode) { _mcp23x17->pinMode(_pinNum, m void MCP23X17PinInterface::digitalWrite(uint8_t val) { _mcp23x17->digitalWrite(_pinNum, val); -} \ No newline at end of file +} + +float MCP23X17PinInterface::analogRead() { return digitalRead() ? 1.0f : 0.0f; } diff --git a/src/sensorinterface/MCP23X17PinInterface.h b/src/sensorinterface/MCP23X17PinInterface.h index ed13a53d9..5df096e2b 100644 --- a/src/sensorinterface/MCP23X17PinInterface.h +++ b/src/sensorinterface/MCP23X17PinInterface.h @@ -24,7 +24,8 @@ #define _H_MCP23X17PinInterface_ #include -#include + +#include "PinInterface.h" #define MCP_GPA0 0 #define MCP_GPA1 1 @@ -55,6 +56,7 @@ class MCP23X17PinInterface : public PinInterface { int digitalRead() override final; void pinMode(uint8_t mode) override final; void digitalWrite(uint8_t val) override final; + float analogRead() override final; private: Adafruit_MCP23X17* _mcp23x17; diff --git a/src/sensorinterface/ParallelMuxInterface.cpp b/src/sensorinterface/ParallelMuxInterface.cpp new file mode 100644 index 000000000..24d60ea32 --- /dev/null +++ b/src/sensorinterface/ParallelMuxInterface.cpp @@ -0,0 +1,101 @@ +/* + 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 "ParallelMuxInterface.h" + +#include + +#include + +namespace SlimeVR { + +ParallelMuxInterface::ParallelMuxInterface( + PinInterface* dataPin, + std::vector& addressPins, + PinInterface* enablePin, + bool enableActiveLevel, + bool addressActiveLevel +) + : dataPin{dataPin} + , addressPins{addressPins} + , enablePin{enablePin} + , enableActiveLevel{enableActiveLevel} + , addressActiveLevel{addressActiveLevel} { + assert(addressPins.size() <= 8); +} + +bool ParallelMuxInterface::init() { + if (enablePin != nullptr) { + enablePin->pinMode(OUTPUT); + enablePin->digitalWrite(enableActiveLevel); + } + + for (auto* pin : addressPins) { + pin->pinMode(OUTPUT); + pin->digitalWrite(false); + } + + return true; +} + +void ParallelMuxInterface::pinMode(uint8_t mode) { dataPin->pinMode(mode); } + +void ParallelMuxInterface::digitalWrite(uint8_t address, uint8_t value) { + switchTo(address); + dataPin->digitalWrite(value); +} + +int ParallelMuxInterface::digitalRead(uint8_t address) { + switchTo(address); + return dataPin->digitalRead(); +} + +float ParallelMuxInterface::analogRead(uint8_t address) { + switchTo(address); + return dataPin->analogRead(); +} + +void ParallelMuxInterface::switchTo(uint8_t address) { + assert(address < 1 << addressPins.size()); + + if (address == currentAddress) { + return; + } + + if (enablePin != nullptr) { + enablePin->digitalWrite(!enableActiveLevel); + } + + for (auto* addressPin : addressPins) { + bool value = address & 0x01; + address >>= 1; + + addressPin->digitalWrite(value); + } + + if (enablePin != nullptr) { + enablePin->digitalWrite(enableActiveLevel); + } + + currentAddress = address; + delay(1); +} + +} // namespace SlimeVR diff --git a/src/sensorinterface/ParallelMuxInterface.h b/src/sensorinterface/ParallelMuxInterface.h new file mode 100644 index 000000000..b678d0c46 --- /dev/null +++ b/src/sensorinterface/ParallelMuxInterface.h @@ -0,0 +1,57 @@ +/* + 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 + +namespace SlimeVR { + +class ParallelMuxInterface { +public: + ParallelMuxInterface( + PinInterface* dataPin, + std::vector& addressPins, + PinInterface* enablePin = nullptr, + bool enableActiveLevel = false, + bool addressActiveLevel = true + ); + + bool init(); + void pinMode(uint8_t mode); + void digitalWrite(uint8_t address, uint8_t value); + int digitalRead(uint8_t address); + float analogRead(uint8_t address); + +private: + void switchTo(uint8_t address); + + PinInterface* const dataPin; + const std::vector addressPins; + PinInterface* const enablePin = nullptr; + const bool enableActiveLevel = false; + const bool addressActiveLevel = true; + uint8_t currentAddress = 0; +}; + +} // namespace SlimeVR diff --git a/src/sensorinterface/ParallelMuxPin.cpp b/src/sensorinterface/ParallelMuxPin.cpp new file mode 100644 index 000000000..6ff96c22d --- /dev/null +++ b/src/sensorinterface/ParallelMuxPin.cpp @@ -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. +*/ + +#include "ParallelMuxPin.h" + +namespace SlimeVR { + +ParallelMuxPin::ParallelMuxPin(ParallelMuxInterface* mux, uint8_t address) + : mux{mux} + , address{address} {} + +void ParallelMuxPin::pinMode(uint8_t mode) { mux->pinMode(mode); } +void ParallelMuxPin::digitalWrite(uint8_t value) { mux->digitalWrite(address, value); } +int ParallelMuxPin::digitalRead() { return mux->digitalRead(address); } +float ParallelMuxPin::analogRead() { return mux->analogRead(address); } + +} // namespace SlimeVR diff --git a/src/sensorinterface/ParallelMuxPin.h b/src/sensorinterface/ParallelMuxPin.h new file mode 100644 index 000000000..d308f455c --- /dev/null +++ b/src/sensorinterface/ParallelMuxPin.h @@ -0,0 +1,43 @@ +/* + 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 "PinInterface.h" +#include "sensorinterface/ParallelMuxInterface.h" +namespace SlimeVR { + +class ParallelMuxPin : public PinInterface { +public: + ParallelMuxPin(ParallelMuxInterface* mux, uint8_t address); + + void pinMode(uint8_t mode) final; + void digitalWrite(uint8_t value) final; + int digitalRead() final; + float analogRead() final; + +private: + ParallelMuxInterface* const mux; + uint8_t address; +}; + +} // namespace SlimeVR diff --git a/src/sensorinterface/SensorInterfaceManager.h b/src/sensorinterface/SensorInterfaceManager.h index 3c66f188b..2b6ac940f 100644 --- a/src/sensorinterface/SensorInterfaceManager.h +++ b/src/sensorinterface/SensorInterfaceManager.h @@ -23,22 +23,44 @@ #pragma once +#include + #include #include #include +#include "ADS111xInterface.h" +#include "ADS111xPin.h" +#include "Adafruit_MCP23X17.h" #include "DirectPinInterface.h" #include "I2CPCAInterface.h" #include "I2CWireSensorInterface.h" #include "MCP23X17PinInterface.h" +#include "ParallelMuxInterface.h" +#include "SensorInterface.h" +#include "sensorinterface/ParallelMuxPin.h" namespace SlimeVR { +template +static bool operator<(std::initializer_list& lhs, std::initializer_list& rhs) { + size_t minLength = std::min(lhs.size(), rhs.size()); + for (size_t i = 0; i < minLength; i++) { + if (lhs[i] < rhs[i]) { + return true; + } + if (lhs[i] > rhs[i]) { + return false; + } + } + return lhs.size() < rhs.size(); +} + class SensorInterfaceManager { private: template - struct SensorInterface { - explicit SensorInterface( + struct InterfaceCache { + explicit InterfaceCache( std::function validate = [](Args...) { return true; } ) : validate{validate} {} @@ -72,14 +94,31 @@ class SensorInterfaceManager { inline auto& mcpPinInterface() { return mcpPinInterfaces; } inline auto& i2cWireInterface() { return i2cWireInterfaces; } inline auto& pcaWireInterface() { return pcaWireInterfaces; } + inline auto& adsInterface() { return adsInterfaces; } + inline auto& adsPinInterface() { return adsPinInterfaces; } + inline auto& parallelMuxInterface() { return parallelMuxInterfaces; } + inline auto& parallelMuxPinInterface() { return parallelMuxPinInterfaces; } private: - SensorInterface directPinInterfaces{[](int pin) { + InterfaceCache directPinInterfaces{[](int pin) { return pin != 255 && pin != -1; }}; - SensorInterface mcpPinInterfaces; - SensorInterface i2cWireInterfaces; - SensorInterface pcaWireInterfaces; + InterfaceCache mcpPinInterfaces; + InterfaceCache i2cWireInterfaces; + InterfaceCache pcaWireInterfaces; + InterfaceCache + adsInterfaces; + InterfaceCache adsPinInterfaces; + InterfaceCache< + ParallelMuxInterface, + PinInterface*, + std::vector, + PinInterface*, + bool, + bool> + parallelMuxInterfaces; + InterfaceCache + parallelMuxPinInterfaces; }; } // namespace SlimeVR diff --git a/src/sensors/ADCResistanceSensor.cpp b/src/sensors/ADCResistanceSensor.cpp index 13cc8168e..840af04db 100644 --- a/src/sensors/ADCResistanceSensor.cpp +++ b/src/sensors/ADCResistanceSensor.cpp @@ -24,18 +24,39 @@ #include "GlobalVars.h" +ADCResistanceSensor::ADCResistanceSensor( + uint8_t id, + float resistanceDivider, + PinInterface* pinInterface, + float smoothFactor +) + : Sensor("ADCResistanceSensor", SensorTypeID::ADC_RESISTANCE, id, 0, 0.0f, nullptr) + , m_PinInterface(pinInterface) + , m_ResistanceDivider(resistanceDivider) + , m_SmoothFactor(smoothFactor) { + working = true; + hadData = true; + lastSampleMicros = micros(); +}; + +void ADCResistanceSensor::motionSetup() { m_PinInterface->pinMode(INPUT); } + void ADCResistanceSensor::motionLoop() { -#if ESP8266 - float voltage = ((float)analogRead(m_Pin)) * ADCVoltageMax / ADCResolution; - m_Data = m_ResistanceDivider - * (ADCVoltageMax / voltage - 1.0f); // Convert voltage to resistance -#elif ESP32 - float voltage = ((float)analogReadMilliVolts(m_Pin)) / 1000; - m_Data = m_ResistanceDivider - * (m_VCC / voltage - 1.0f); // Convert voltage to resistance -#endif + if (micros() - lastSampleMicros < samplingStepMicros) { + return; + } + float value = m_PinInterface->analogRead(); + m_Data = m_ResistanceDivider * value; + lastSampleMicros += samplingStepMicros; + hasNewSample = true; } void ADCResistanceSensor::sendData() { + if (!hasNewSample) { + return; + } networkConnection.sendFlexData(sensorId, m_Data); + hasNewSample = false; } + +bool ADCResistanceSensor::hasNewDataToSend() { return hasNewSample; } diff --git a/src/sensors/ADCResistanceSensor.h b/src/sensors/ADCResistanceSensor.h index cd4a40f65..1f3cf9b8f 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -22,36 +22,27 @@ */ #pragma once +#include + +#include "../sensorinterface/SensorInterface.h" #include "sensor.h" -#include "sensorinterface/SensorInterface.h" -class ADCResistanceSensor : Sensor { +class ADCResistanceSensor : public Sensor { public: static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE; ADCResistanceSensor( uint8_t id, - uint8_t pin, - float VCC, float resistanceDivider, + PinInterface* pinInterface = nullptr, float smoothFactor = 0.1f - ) - : Sensor( - "ADCResistanceSensor", - SensorTypeID::ADC_RESISTANCE, - id, - pin, - 0.0f, - new SlimeVR::EmptySensorInterface - ) - , m_Pin(pin) - , m_VCC(VCC) - , m_ResistanceDivider(resistanceDivider) - , m_SmoothFactor(smoothFactor){}; - ~ADCResistanceSensor(); + ); + ~ADCResistanceSensor() = default; - void motionLoop() override final; - void sendData() override final; + void motionSetup() final; + void motionLoop() final; + void sendData() final; + bool hasNewDataToSend() final; SensorStatus getSensorState() override final { return SensorStatus::SENSOR_OK; } @@ -60,10 +51,14 @@ class ADCResistanceSensor : Sensor { }; private: - uint8_t m_Pin; - float m_VCC; + static constexpr uint32_t samplingRateHz = 120; + static constexpr uint64_t samplingStepMicros = 1000'000 / samplingRateHz; + + PinInterface* m_PinInterface; float m_ResistanceDivider; float m_SmoothFactor; + uint64_t lastSampleMicros = 0; + bool hasNewSample = false; float m_Data = 0.0f; }; diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 505b17afc..d5187455e 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -23,15 +23,21 @@ #include "SensorManager.h" +#include #include #include +#include "../sensorinterface/ADS111xInterface.h" +#include "../sensorinterface/ADS111xPin.h" +#include "ADCResistanceSensor.h" +#include "PinInterface.h" #include "bmi160sensor.h" #include "bno055sensor.h" #include "bno080sensor.h" #include "icm20948sensor.h" #include "mpu6050sensor.h" #include "mpu9250sensor.h" +#include "sensorinterface/SensorInterface.h" #include "sensorinterface/SensorInterfaceManager.h" #include "sensors/softfusion/SoftfusionCalibration.h" #include "sensors/softfusion/runtimecalibration/RuntimeCalibration.h" @@ -95,12 +101,47 @@ void SensorManager::setup() { 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); + [[maybe_unused]] static constexpr auto* NO_PIN + = static_cast(nullptr); + [[maybe_unused]] static auto DIRECT_PIN = [&](uint8_t pin) constexpr { + return interfaceManager.directPinInterface().get(pin); + }; + [[maybe_unused]] static auto DIRECT_WIRE = [&](uint8_t scl, uint8_t sda) constexpr { + return interfaceManager.i2cWireInterface().get(scl, sda); + }; + [[maybe_unused]] static auto MCP_PIN = [&](uint8_t pin) constexpr { + return interfaceManager.mcpPinInterface().get(&m_MCP, pin); + }; + [[maybe_unused]] static auto PCA_WIRE + = [&](uint8_t scl, uint8_t sda, uint8_t addr, uint8_t ch) constexpr { + return interfaceManager.pcaWireInterface().get(scl, sda, addr, ch); + }; + [[maybe_unused]] static auto ADS_PIN + = [&](SensorInterface* interface, PinInterface* drdy, uint8_t addr, uint8_t ch + ) constexpr { + return interfaceManager.adsPinInterface().get( + interfaceManager.adsInterface().get(interface, drdy, addr), + ch + ); + }; + [[maybe_unused]] static auto MUX_PIN + = [&](PinInterface* data, + std::vector&& addressPins, + uint8_t channel, + PinInterface* enablePin = nullptr, + bool enableActiveLevel = false, + bool addressActiveLevel = true) constexpr { + return interfaceManager.parallelMuxPinInterface().get( + interfaceManager.parallelMuxInterface().get( + data, + addressPins, + enablePin, + enableActiveLevel, + addressActiveLevel + ), + channel + ); + }; #define SENSOR_DESC_ENTRY(ImuType, ...) \ { \ @@ -116,7 +157,7 @@ void SensorManager::setup() { // Apply descriptor list and expand to entries SENSOR_DESC_LIST; -#define SENSOR_INFO_ENTRY(ImuID, ...) \ +#define SENSOR_INFO_ENTRY(SensorTypeID, ...) \ { m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); } SENSOR_INFO_LIST; @@ -124,6 +165,10 @@ void SensorManager::setup() { #undef NO_PIN #undef DIRECT_PIN #undef DIRECT_WIRE +#undef MCP_PIN +#undef PCA_WIRE +#undef ADS_PIN + m_Logger.info("%d sensor(s) configured", activeSensorCount); // Check and scan i2c if no sensors active if (activeSensorCount == 0) { diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 8be7a097b..c5361fcb4 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -154,6 +154,35 @@ class SensorManager { return sensor; } + // ADC Sensors + template + std::unique_ptr<::Sensor> buildSensor( + uint8_t sensorId, + float voltageDivider, + PinInterface* pinInterface, + float smoothingFactor + ) { + m_Logger.trace( + "Building Sensor with: id=%d,\n\ + voltage divider=%f, smoothing factor=%f\n\ + interface=%s", + sensorId, + voltageDivider, + smoothingFactor, + pinInterface + ); + + std::unique_ptr<::Sensor> sensor = std::make_unique( + sensorId, + voltageDivider, + pinInterface, + smoothingFactor + ); + + sensor->motionSetup(); + return sensor; + } + uint32_t m_LastBundleSentAtMicros = micros(); }; } // namespace Sensors diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index b0aa0f417..dec06445b 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -25,6 +25,7 @@ #define SENSORS_BMI160SENSOR_H #include +#include #include "../motionprocessing/GyroTemperatureCalibrator.h" #include "../motionprocessing/RestDetection.h" diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 16fb6b020..91f8fa9a2 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -25,6 +25,7 @@ #define SENSORS_BNO055SENSOR_H #include +#include #include "sensor.h" diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 469b67160..d9f3f3206 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -24,6 +24,7 @@ #define SLIMEVR_ICM20948SENSOR_H_ #include +#include #include "SensorFusionDMP.h" #include "sensor.h" diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index b8e9d5724..f54cf0baa 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -25,6 +25,7 @@ #define SENSORS_MPU9250SENSOR_H #include +#include #include "logging/Logger.h" #include "sensor.h" diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 3e66e0c82..c800a4d3f 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -141,6 +141,8 @@ const char* getIMUNameByType(SensorTypeID imuType) { return "ICM45686"; case SensorTypeID::ICM45605: return "ICM45605"; + case SensorTypeID::ADC_RESISTANCE: + return "Flex Sensor"; case SensorTypeID::Unknown: case SensorTypeID::Empty: return "UNKNOWN"; diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 81c8e7912..f9cfe9e12 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -89,7 +89,7 @@ class Sensor { SensorTypeID getSensorType() { return sensorType; }; const Vector3& getAcceleration() { return acceleration; }; const Quat& getFusedRotation() { return fusedRotation; }; - bool hasNewDataToSend() { return newFusedRotation || newAcceleration; }; + virtual bool hasNewDataToSend() { return newFusedRotation || newAcceleration; }; inline bool hasCompletedRestCalibration() { return restCalibrationComplete; } void setFlag(SensorToggles toggle, bool state); [[nodiscard]] virtual bool isFlagSupported(SensorToggles toggle) const {