From 7e99feeae528eb8355881ff1049f68387001529b Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Fri, 21 Mar 2025 05:53:57 +0100 Subject: [PATCH 1/9] Initial ADS111x implementation --- lib/bno080/BNO080.h | 2 +- lib/bno080/PinInterface.h | 45 ++++++----- src/batterymonitor.h | 12 +-- src/consts.h | 15 ++++ src/sensorinterface/ADS111xInterface.cpp | 81 ++++++++++++++++++++ src/sensorinterface/ADS111xInterface.h | 77 +++++++++++++++++++ src/sensorinterface/DirectPinInterface.cpp | 12 ++- src/sensorinterface/DirectPinInterface.h | 4 +- src/sensorinterface/MCP23X17PinInterface.cpp | 4 +- src/sensorinterface/MCP23X17PinInterface.h | 4 +- src/sensors/ADCResistanceSensor.cpp | 11 +-- src/sensors/ADCResistanceSensor.h | 19 +++-- src/sensors/SensorManager.cpp | 25 +++++- src/sensors/bmi160sensor.h | 1 + src/sensors/bno055sensor.h | 1 + src/sensors/icm20948sensor.h | 1 + src/sensors/mpu9250sensor.h | 1 + src/sensors/sensor.cpp | 2 + src/sensors/sensor.h | 1 - 19 files changed, 255 insertions(+), 63 deletions(-) create mode 100644 src/sensorinterface/ADS111xInterface.cpp create mode 100644 src/sensorinterface/ADS111xInterface.h diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index e7f318cbd..816e920e2 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 e3a0ab91f..ff7f88726 100644 --- a/lib/bno080/PinInterface.h +++ b/lib/bno080/PinInterface.h @@ -1,34 +1,33 @@ /* - 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 int digitalRead() = 0; - virtual void pinMode(uint8_t mode) = 0; - virtual void digitalWrite(uint8_t val) = 0; - + 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 b985072b7..6c467f76d 100644 --- a/src/consts.h +++ b/src/consts.h @@ -172,6 +172,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..b9e8752ee --- /dev/null +++ b/src/sensorinterface/ADS111xInterface.cpp @@ -0,0 +1,81 @@ +#include "ADS111xInterface.h" + +namespace SlimeVR { + +ADS111xInterface::ADS111xInterface( + uint8_t sclPin, + uint8_t sdaPin, + uint8_t drdyPin, + uint8_t address, + uint8_t channel +) + : wire(sclPin, sdaPin) + , address(address) + , channel(channel) + , drdy(drdyPin) {} + +bool ADS111xInterface::init() { + wire.swapIn(); + Wire.beginTransmission(address); + Wire.write(Registers::Config::Addr); + Registers::Config config{ + .os = 0b0, // Don't start read + .mux = 0b000, // Doesn't matter + .pga = 0b010, // Gain (FSR): 2.048V + .mode = 0b1, // Single-shot + .dr = 0b100, // Data-rate: 128 samples per second + .compMode = 0b0, // Traditional comparator + .compPol = 0b0, // Active low drdy + .compLat = 0b1, // Latching drdy + .compQue = 0b00, // Assert drdy after every sample + }; + uint8_t* bytes = reinterpret_cast(&config); + Wire.write(bytes[1]); + Wire.write(bytes[0]); + Wire.endTransmission(); + drdy.pinMode(INPUT_PULLUP); + return true; +} + +int ADS111xInterface::digitalRead() { return analogRead() >= 0.5f; } + +void ADS111xInterface::pinMode(uint8_t mode) {} + +void ADS111xInterface::digitalWrite(uint8_t val) {} + +float ADS111xInterface::analogRead() { + wire.swapIn(); + Wire.beginTransmission(address); + Wire.write(Registers::Config::Addr); + Registers::Config config{ + .os = 0b1, // Start read + .mux = static_cast(0b100 | channel), // Current channel + .pga = 0b010, // Gain (FSR): 2.048V + .mode = 0b1, // Single-shot + .dr = 0b100, // Data-rate: 128 samples per second + .compMode = 0b0, // Traditional comparator + .compPol = 0b0, // Active low drdy + .compLat = 0b0, // Non-latching drdy + .compQue = 0b11, // Comparator disabled + }; + uint8_t* bytes = reinterpret_cast(&config); + Wire.write(bytes[1]); + Wire.write(bytes[0]); + Wire.endTransmission(); + + // Wait for drdy signal + while (drdy.digitalRead()) + ; + + Wire.beginTransmission(address); + Wire.write(Registers::ConversionAddr); + Wire.endTransmission(); + + uint8_t msb = Wire.read(); + uint8_t lsb = Wire.read(); + + uint16_t value = (msb << 8) | lsb; + return static_cast(value) / maxValue; +} + +} // namespace SlimeVR diff --git a/src/sensorinterface/ADS111xInterface.h b/src/sensorinterface/ADS111xInterface.h new file mode 100644 index 000000000..53f554f1f --- /dev/null +++ b/src/sensorinterface/ADS111xInterface.h @@ -0,0 +1,77 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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 "DirectPinInterface.h" +#include "I2CWireSensorInterface.h" +#include "SensorInterface.h" + +namespace SlimeVR { + +class ADS111xInterface : public PinInterface { +public: + explicit ADS111xInterface( + uint8_t sclPin, + uint8_t sdaPin, + uint8_t drdyPin, + uint8_t address, + uint8_t channel + ); + bool init(); + int digitalRead() override final; + void pinMode(uint8_t mode) override final; + void digitalWrite(uint8_t val); + float analogRead(); + +private: + static constexpr uint32_t maxValue = 0x7fff; + + struct Registers { + static constexpr uint8_t ConversionAddr = 0b00; + struct Config { + static constexpr uint8_t Addr = 0b01; + uint8_t os : 1; + uint8_t mux : 3; + uint8_t pga : 3; + uint8_t mode : 1; + uint8_t dr : 3; + uint8_t compMode : 1; + uint8_t compPol : 1; + uint8_t compLat : 1; + uint8_t compQue : 2; + }; + }; + static_assert(sizeof(Registers::Config) == 2); + + I2CWireSensorInterface wire; + uint8_t address; + uint8_t channel; + DirectPinInterface drdy; +}; + +} // 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..e88746170 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 @@ -38,6 +39,7 @@ class DirectPinInterface : 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: 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/sensors/ADCResistanceSensor.cpp b/src/sensors/ADCResistanceSensor.cpp index 13cc8168e..2077e0831 100644 --- a/src/sensors/ADCResistanceSensor.cpp +++ b/src/sensors/ADCResistanceSensor.cpp @@ -25,15 +25,8 @@ #include "GlobalVars.h" 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 + float value = m_PinInterface->analogRead(); + m_Data = m_ResistanceDivider * (value - 1.0f); } void ADCResistanceSensor::sendData() { diff --git a/src/sensors/ADCResistanceSensor.h b/src/sensors/ADCResistanceSensor.h index cd4a40f65..41ebdf7ac 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -22,8 +22,10 @@ */ #pragma once +#include + +#include "../sensorinterface/SensorInterface.h" #include "sensor.h" -#include "sensorinterface/SensorInterface.h" class ADCResistanceSensor : Sensor { public: @@ -31,23 +33,21 @@ class ADCResistanceSensor : Sensor { 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, 0.0f, - new SlimeVR::EmptySensorInterface + new SlimeVR::EmptySensorInterface() ) - , m_Pin(pin) - , m_VCC(VCC) , m_ResistanceDivider(resistanceDivider) - , m_SmoothFactor(smoothFactor){}; + , m_SmoothFactor(smoothFactor) + , m_PinInterface(pinInterface){}; ~ADCResistanceSensor(); void motionLoop() override final; @@ -60,8 +60,7 @@ class ADCResistanceSensor : Sensor { }; private: - uint8_t m_Pin; - float m_VCC; + PinInterface* m_PinInterface; float m_ResistanceDivider; float m_SmoothFactor; diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 64ec1031c..4052faac3 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -26,6 +26,7 @@ #include #include +#include "../sensorinterface/ADS111xInterface.h" #include "bmi160sensor.h" #include "bno055sensor.h" #include "bno080sensor.h" @@ -92,8 +93,9 @@ void SensorManager::setup() { std::map mcpPinInterfaces; std::map, I2CWireSensorInterface*> i2cWireInterfaces; std::map, I2CPCASensorInterface*> pcaWireInterfaces; + std::map, ADS111xInterface*> adsPinInterfaces; - auto directPin = [&](int pin) { + [[maybe_unused]] auto directPin = [&](int pin) { if (pin == 255 || pin == -1) { return static_cast(nullptr); } @@ -104,7 +106,7 @@ void SensorManager::setup() { return directPinInterfaces[pin]; }; - auto mcpPin = [&](int pin) { + [[maybe_unused]] auto mcpPin = [&](int pin) { if (!mcpPinInterfaces.contains(pin)) { auto ptr = new MCP23X17PinInterface(&m_MCP, pin); mcpPinInterfaces[pin] = ptr; @@ -112,7 +114,7 @@ void SensorManager::setup() { return mcpPinInterfaces[pin]; }; - auto directWire = [&](int scl, int sda) { + [[maybe_unused]] auto directWire = [&](int scl, int sda) { auto pair = std::make_tuple(scl, sda); if (!i2cWireInterfaces.contains(pair)) { auto ptr = new I2CWireSensorInterface(scl, sda); @@ -121,7 +123,7 @@ void SensorManager::setup() { return i2cWireInterfaces[pair]; }; - auto pcaWire = [&](int scl, int sda, int addr, int ch) { + [[maybe_unused]] auto pcaWire = [&](int scl, int sda, int addr, int ch) { auto pair = std::make_tuple(scl, sda, addr, ch); if (!pcaWireInterfaces.contains(pair)) { auto ptr = new I2CPCASensorInterface(scl, sda, addr, ch); @@ -129,6 +131,16 @@ void SensorManager::setup() { } return pcaWireInterfaces[pair]; }; + + [[maybe_unused]] auto adsPin = [&](int scl, int sda, int drdy, int addr, int ch) { + auto pair = std::make_tuple(scl, sda, drdy, addr, ch); + if (!adsPinInterfaces.contains(pair)) { + auto ptr = new ADS111xInterface(scl, sda, drdy, addr, ch); + adsPinInterfaces[pair] = ptr; + ptr->init(); + } + return adsPinInterfaces[pair]; + }; uint8_t sensorID = 0; uint8_t activeSensorCount = 0; if (m_MCP.begin_I2C()) { @@ -140,6 +152,7 @@ void SensorManager::setup() { #define DIRECT_WIRE(scl, sda) directWire(scl, sda) #define MCP_PIN(pin) mcpPin(pin) #define PCA_WIRE(scl, sda, addr, ch) pcaWire(scl, sda, addr, ch) +#define ADS_PIN(scl, sda, drdy, addr, ch) adsPin(scl, sda, drdy, addr, ch) #define SENSOR_DESC_ENTRY(ImuType, ...) \ { \ @@ -163,6 +176,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/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 4e2a361f4..f82394b60 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -135,6 +135,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 b89d87c91..1ae419799 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -30,7 +30,6 @@ #include -#include "PinInterface.h" #include "configuration/Configuration.h" #include "globals.h" #include "logging/Logger.h" From 91da63f1d99804dd3ff41663f578eae351b9cd45 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Fri, 21 Mar 2025 13:37:53 +0100 Subject: [PATCH 2/9] Refactor ADS implementation to use generic sensor and pininterfaces instead --- src/sensorinterface/ADS111xInterface.cpp | 19 +++++++++---------- src/sensorinterface/ADS111xInterface.h | 9 ++++----- src/sensors/SensorManager.cpp | 22 ++++++++++++---------- 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/sensorinterface/ADS111xInterface.cpp b/src/sensorinterface/ADS111xInterface.cpp index b9e8752ee..1c8e3d941 100644 --- a/src/sensorinterface/ADS111xInterface.cpp +++ b/src/sensorinterface/ADS111xInterface.cpp @@ -3,19 +3,18 @@ namespace SlimeVR { ADS111xInterface::ADS111xInterface( - uint8_t sclPin, - uint8_t sdaPin, - uint8_t drdyPin, + SensorInterface* interface, + PinInterface* drdy, uint8_t address, uint8_t channel ) - : wire(sclPin, sdaPin) + : interface(interface) , address(address) , channel(channel) - , drdy(drdyPin) {} + , drdy(drdy) {} bool ADS111xInterface::init() { - wire.swapIn(); + interface->swapIn(); Wire.beginTransmission(address); Wire.write(Registers::Config::Addr); Registers::Config config{ @@ -33,7 +32,7 @@ bool ADS111xInterface::init() { Wire.write(bytes[1]); Wire.write(bytes[0]); Wire.endTransmission(); - drdy.pinMode(INPUT_PULLUP); + drdy->pinMode(INPUT_PULLUP); return true; } @@ -44,7 +43,7 @@ void ADS111xInterface::pinMode(uint8_t mode) {} void ADS111xInterface::digitalWrite(uint8_t val) {} float ADS111xInterface::analogRead() { - wire.swapIn(); + interface->swapIn(); Wire.beginTransmission(address); Wire.write(Registers::Config::Addr); Registers::Config config{ @@ -58,13 +57,13 @@ float ADS111xInterface::analogRead() { .compLat = 0b0, // Non-latching drdy .compQue = 0b11, // Comparator disabled }; - uint8_t* bytes = reinterpret_cast(&config); + auto* bytes = reinterpret_cast(&config); Wire.write(bytes[1]); Wire.write(bytes[0]); Wire.endTransmission(); // Wait for drdy signal - while (drdy.digitalRead()) + while (drdy->digitalRead()) ; Wire.beginTransmission(address); diff --git a/src/sensorinterface/ADS111xInterface.h b/src/sensorinterface/ADS111xInterface.h index 53f554f1f..255b73e72 100644 --- a/src/sensorinterface/ADS111xInterface.h +++ b/src/sensorinterface/ADS111xInterface.h @@ -36,9 +36,8 @@ namespace SlimeVR { class ADS111xInterface : public PinInterface { public: explicit ADS111xInterface( - uint8_t sclPin, - uint8_t sdaPin, - uint8_t drdyPin, + SensorInterface* interface, + PinInterface* drdy, uint8_t address, uint8_t channel ); @@ -68,10 +67,10 @@ class ADS111xInterface : public PinInterface { }; static_assert(sizeof(Registers::Config) == 2); - I2CWireSensorInterface wire; + SensorInterface* interface; uint8_t address; uint8_t channel; - DirectPinInterface drdy; + PinInterface* drdy; }; } // namespace SlimeVR diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 4052faac3..450bd98c8 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -93,7 +93,8 @@ void SensorManager::setup() { std::map mcpPinInterfaces; std::map, I2CWireSensorInterface*> i2cWireInterfaces; std::map, I2CPCASensorInterface*> pcaWireInterfaces; - std::map, ADS111xInterface*> adsPinInterfaces; + std::map, ADS111xInterface*> + adsPinInterfaces; [[maybe_unused]] auto directPin = [&](int pin) { if (pin == 255 || pin == -1) { @@ -132,15 +133,16 @@ void SensorManager::setup() { return pcaWireInterfaces[pair]; }; - [[maybe_unused]] auto adsPin = [&](int scl, int sda, int drdy, int addr, int ch) { - auto pair = std::make_tuple(scl, sda, drdy, addr, ch); - if (!adsPinInterfaces.contains(pair)) { - auto ptr = new ADS111xInterface(scl, sda, drdy, addr, ch); - adsPinInterfaces[pair] = ptr; - ptr->init(); - } - return adsPinInterfaces[pair]; - }; + [[maybe_unused]] auto adsPin + = [&](SensorInterface* interface, PinInterface* drdy, int addr, int ch) { + auto pair = std::make_tuple(interface, drdy, addr, ch); + if (!adsPinInterfaces.contains(pair)) { + auto ptr = new ADS111xInterface(interface, drdy, addr, ch); + adsPinInterfaces[pair] = ptr; + ptr->init(); + } + return adsPinInterfaces[pair]; + }; uint8_t sensorID = 0; uint8_t activeSensorCount = 0; if (m_MCP.begin_I2C()) { From bd80446cb6e58c23115949d921458d54af5134ff Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 5 Apr 2025 02:06:38 +0200 Subject: [PATCH 3/9] Make everything work --- src/sensorinterface/ADS111xInterface.cpp | 85 ++++++++++++------------ src/sensorinterface/ADS111xInterface.h | 41 ++++++------ src/sensorinterface/ADS111xPin.cpp | 19 ++++++ src/sensorinterface/ADS111xPin.h | 46 +++++++++++++ src/sensors/ADCResistanceSensor.cpp | 28 +++++++- src/sensors/ADCResistanceSensor.h | 27 +++----- src/sensors/SensorManager.cpp | 44 ++++++++---- src/sensors/SensorManager.h | 29 ++++++++ src/sensors/sensor.h | 2 +- 9 files changed, 224 insertions(+), 97 deletions(-) create mode 100644 src/sensorinterface/ADS111xPin.cpp create mode 100644 src/sensorinterface/ADS111xPin.h diff --git a/src/sensorinterface/ADS111xInterface.cpp b/src/sensorinterface/ADS111xInterface.cpp index 1c8e3d941..32a0a127d 100644 --- a/src/sensorinterface/ADS111xInterface.cpp +++ b/src/sensorinterface/ADS111xInterface.cpp @@ -1,79 +1,78 @@ #include "ADS111xInterface.h" +#include + namespace SlimeVR { -ADS111xInterface::ADS111xInterface( - SensorInterface* interface, - PinInterface* drdy, - uint8_t address, - uint8_t channel -) - : interface(interface) - , address(address) - , channel(channel) - , drdy(drdy) {} +ADS111xInterface::ADS111xInterface(SensorInterface* interface, uint8_t address) + : interface { + interface +}, address{address} { +} bool ADS111xInterface::init() { interface->swapIn(); Wire.beginTransmission(address); - Wire.write(Registers::Config::Addr); + Wire.write(static_cast(Registers::Addresses::Config)); Registers::Config config{ - .os = 0b0, // Don't start read - .mux = 0b000, // Doesn't matter - .pga = 0b010, // Gain (FSR): 2.048V .mode = 0b1, // Single-shot - .dr = 0b100, // Data-rate: 128 samples per second - .compMode = 0b0, // Traditional comparator - .compPol = 0b0, // Active low drdy - .compLat = 0b1, // Latching drdy - .compQue = 0b00, // Assert drdy after every sample + .pga = 0b010, // Gain (FSR): 2.048V + .mux = 0b000, // Doesn't matter + .os = 0b0, // Don't start read + .compQue = 0b11, // Disable comparator + .compLat = 0b0, // Doesn't matter + .compPol = 0b0, // Doesn't matter + .compMode = 0b0, // Doesn't matter + .dr = 0b100, // Doesn't matter }; uint8_t* bytes = reinterpret_cast(&config); - Wire.write(bytes[1]); Wire.write(bytes[0]); - Wire.endTransmission(); - drdy->pinMode(INPUT_PULLUP); - return true; -} - -int ADS111xInterface::digitalRead() { return analogRead() >= 0.5f; } + Wire.write(bytes[1]); + auto result = Wire.endTransmission(); -void ADS111xInterface::pinMode(uint8_t mode) {} + if (result != 0) { + logger.error("Couldn't initialize ADS interface!\n"); + return false; + } -void ADS111xInterface::digitalWrite(uint8_t val) {} + return true; +} -float ADS111xInterface::analogRead() { +float ADS111xInterface::read(uint8_t channel) { interface->swapIn(); Wire.beginTransmission(address); - Wire.write(Registers::Config::Addr); + Wire.write(static_cast(Registers::Addresses::Config)); Registers::Config config{ - .os = 0b1, // Start read - .mux = static_cast(0b100 | channel), // Current channel - .pga = 0b010, // Gain (FSR): 2.048V .mode = 0b1, // Single-shot - .dr = 0b100, // Data-rate: 128 samples per second - .compMode = 0b0, // Traditional comparator - .compPol = 0b0, // Active low drdy - .compLat = 0b0, // Non-latching drdy - .compQue = 0b11, // Comparator disabled + .pga = 0b010, // Gain (FSR): 2.048V + .mux = static_cast(0b100 | channel), // Current channel + .os = 0b1, // Start read + .compQue = 0b11, // Disable comparator + .compLat = 0b0, // Doesn't matter + .compPol = 0b0, // Doesn't matter + .compMode = 0b0, // Doesn't matter + .dr = 0b111, // 860 samples per second }; + auto* bytes = reinterpret_cast(&config); - Wire.write(bytes[1]); Wire.write(bytes[0]); + Wire.write(bytes[1]); Wire.endTransmission(); - // Wait for drdy signal - while (drdy->digitalRead()) - ; + delayMicroseconds(1e6 / 860); Wire.beginTransmission(address); - Wire.write(Registers::ConversionAddr); + 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(); uint16_t value = (msb << 8) | lsb; + return static_cast(value) / maxValue; } diff --git a/src/sensorinterface/ADS111xInterface.h b/src/sensorinterface/ADS111xInterface.h index 255b73e72..d1dbabaac 100644 --- a/src/sensorinterface/ADS111xInterface.h +++ b/src/sensorinterface/ADS111xInterface.h @@ -27,50 +27,47 @@ #include +#include "../logging/Logger.h" #include "DirectPinInterface.h" #include "I2CWireSensorInterface.h" #include "SensorInterface.h" namespace SlimeVR { -class ADS111xInterface : public PinInterface { +class ADS111xInterface { public: - explicit ADS111xInterface( - SensorInterface* interface, - PinInterface* drdy, - uint8_t address, - uint8_t channel - ); + ADS111xInterface(SensorInterface* interface, uint8_t address); bool init(); - int digitalRead() override final; - void pinMode(uint8_t mode) override final; - void digitalWrite(uint8_t val); - float analogRead(); + + float read(uint8_t channel); private: static constexpr uint32_t maxValue = 0x7fff; struct Registers { - static constexpr uint8_t ConversionAddr = 0b00; + enum class Addresses : uint8_t { + Conversion = 0x00, + Config = 0x01, + }; struct Config { - static constexpr uint8_t Addr = 0b01; - uint8_t os : 1; - uint8_t mux : 3; - uint8_t pga : 3; uint8_t mode : 1; - uint8_t dr : 3; - uint8_t compMode : 1; - uint8_t compPol : 1; - uint8_t compLat : 1; + uint8_t pga : 3; + uint8_t mux : 3; + uint8_t os : 1; uint8_t compQue : 2; + uint8_t compLat : 1; + uint8_t compPol : 1; + uint8_t compMode : 1; + uint8_t dr : 3; }; }; static_assert(sizeof(Registers::Config) == 2); SensorInterface* interface; uint8_t address; - uint8_t channel; - PinInterface* drdy; + uint8_t counter = 0; + + Logging::Logger logger = Logging::Logger("ADS111x"); }; } // namespace SlimeVR diff --git a/src/sensorinterface/ADS111xPin.cpp b/src/sensorinterface/ADS111xPin.cpp new file mode 100644 index 000000000..b12219a71 --- /dev/null +++ b/src/sensorinterface/ADS111xPin.cpp @@ -0,0 +1,19 @@ +#include "ADS111xPin.h" + +#include + +namespace SlimeVR { + +ADS111xPin::ADS111xPin(ADS111xInterface* interface, uint8_t channel) + : ads111x{interface} + , channel{channel} { + assert(channel < 4); +} + +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..8e90e7579 --- /dev/null +++ b/src/sensorinterface/ADS111xPin.h @@ -0,0 +1,46 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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/sensors/ADCResistanceSensor.cpp b/src/sensors/ADCResistanceSensor.cpp index 2077e0831..5e97cff08 100644 --- a/src/sensors/ADCResistanceSensor.cpp +++ b/src/sensors/ADCResistanceSensor.cpp @@ -24,11 +24,37 @@ #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_ResistanceDivider(resistanceDivider) + , m_SmoothFactor(smoothFactor) + , m_PinInterface(pinInterface) { + working = true; + hadData = true; + lastSampleMicros = micros(); +}; + void ADCResistanceSensor::motionLoop() { + if (micros() - lastSampleMicros < samplingStepMicros) { + return; + } float value = m_PinInterface->analogRead(); - m_Data = m_ResistanceDivider * (value - 1.0f); + 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 41ebdf7ac..6867d55c8 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -27,7 +27,7 @@ #include "../sensorinterface/SensorInterface.h" #include "sensor.h" -class ADCResistanceSensor : Sensor { +class ADCResistanceSensor : public Sensor { public: static constexpr auto TypeID = SensorTypeID::ADC_RESISTANCE; @@ -36,22 +36,12 @@ class ADCResistanceSensor : Sensor { float resistanceDivider, PinInterface* pinInterface = nullptr, float smoothFactor = 0.1f - ) - : Sensor( - "ADCResistanceSensor", - SensorTypeID::ADC_RESISTANCE, - id, - 0, - 0.0f, - new SlimeVR::EmptySensorInterface() - ) - , m_ResistanceDivider(resistanceDivider) - , m_SmoothFactor(smoothFactor) - , m_PinInterface(pinInterface){}; - ~ADCResistanceSensor(); + ); + ~ADCResistanceSensor() = default; - void motionLoop() override final; - void sendData() override final; + void motionLoop() final; + void sendData() final; + bool hasNewDataToSend() final; SensorStatus getSensorState() override final { return SensorStatus::SENSOR_OK; } @@ -60,9 +50,14 @@ class ADCResistanceSensor : Sensor { }; private: + static constexpr uint32_t samplingRateHz = 60; + 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 450bd98c8..32352e444 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -27,6 +27,8 @@ #include #include "../sensorinterface/ADS111xInterface.h" +#include "../sensorinterface/ADS111xPin.h" +#include "ADCResistanceSensor.h" #include "bmi160sensor.h" #include "bno055sensor.h" #include "bno080sensor.h" @@ -93,8 +95,8 @@ void SensorManager::setup() { std::map mcpPinInterfaces; std::map, I2CWireSensorInterface*> i2cWireInterfaces; std::map, I2CPCASensorInterface*> pcaWireInterfaces; - std::map, ADS111xInterface*> - adsPinInterfaces; + std::map, ADS111xInterface*> adsInterfaces; + std::map, ADS111xPin*> adsPins; [[maybe_unused]] auto directPin = [&](int pin) { if (pin == 255 || pin == -1) { @@ -133,16 +135,30 @@ void SensorManager::setup() { return pcaWireInterfaces[pair]; }; - [[maybe_unused]] auto adsPin - = [&](SensorInterface* interface, PinInterface* drdy, int addr, int ch) { - auto pair = std::make_tuple(interface, drdy, addr, ch); - if (!adsPinInterfaces.contains(pair)) { - auto ptr = new ADS111xInterface(interface, drdy, addr, ch); - adsPinInterfaces[pair] = ptr; - ptr->init(); - } - return adsPinInterfaces[pair]; - }; + [[maybe_unused]] auto adsInterface = [&](SensorInterface* interface, int addr) { + auto key = std::make_tuple(interface, addr); + if (!adsInterfaces.contains(key)) { + auto ptr = new ADS111xInterface(interface, addr); + if (!ptr->init()) { + return static_cast(nullptr); + } + adsInterfaces[key] = ptr; + } + return adsInterfaces[key]; + }; + + [[maybe_unused]] auto adsPin = [&](SensorInterface* interface, int addr, int ch) { + auto ads = adsInterface(interface, addr); + if (ads == nullptr) { + return static_cast(nullptr); + } + auto key = std::make_tuple(ads, ch); + if (!adsPins.contains(key)) { + auto ptr = new ADS111xPin(ads, ch); + adsPins[key] = ptr; + } + return adsPins[key]; + }; uint8_t sensorID = 0; uint8_t activeSensorCount = 0; if (m_MCP.begin_I2C()) { @@ -154,7 +170,7 @@ void SensorManager::setup() { #define DIRECT_WIRE(scl, sda) directWire(scl, sda) #define MCP_PIN(pin) mcpPin(pin) #define PCA_WIRE(scl, sda, addr, ch) pcaWire(scl, sda, addr, ch) -#define ADS_PIN(scl, sda, drdy, addr, ch) adsPin(scl, sda, drdy, addr, ch) +#define ADS_PIN(interface, addr, ch) adsPin(interface, addr, ch) #define SENSOR_DESC_ENTRY(ImuType, ...) \ { \ @@ -170,7 +186,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; diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 36e877ee3..fd259a7ca 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -146,6 +146,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/sensor.h b/src/sensors/sensor.h index 1ae419799..12cd3e920 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -97,7 +97,7 @@ class Sensor { MagnetometerStatus getMagStatus() { return magStatus; }; 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; } virtual SensorDataType getDataType() { From b2251e62af140afa77c7f05910b8902374e76cc4 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 5 Apr 2025 02:40:34 +0200 Subject: [PATCH 4/9] Formatting --- src/sensorinterface/ADS111xInterface.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/sensorinterface/ADS111xInterface.cpp b/src/sensorinterface/ADS111xInterface.cpp index 32a0a127d..2a8f65343 100644 --- a/src/sensorinterface/ADS111xInterface.cpp +++ b/src/sensorinterface/ADS111xInterface.cpp @@ -5,10 +5,8 @@ namespace SlimeVR { ADS111xInterface::ADS111xInterface(SensorInterface* interface, uint8_t address) - : interface { - interface -}, address{address} { -} + : interface{interface} + , address{address} {} bool ADS111xInterface::init() { interface->swapIn(); From dc6f6d50adaab0fafd794e4a8fa561c9784896a2 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 5 Apr 2025 02:41:24 +0200 Subject: [PATCH 5/9] Consts update --- src/consts.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/consts.h b/src/consts.h index 6c467f76d..7865bf9f6 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 From 04fbc7e33df193748a501c8bf2343cb5c082ae92 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sat, 5 Apr 2025 23:54:23 +0200 Subject: [PATCH 6/9] Use DRDY pin and optimize ADS1115 with only one channel used --- src/sensorinterface/ADS111xInterface.cpp | 115 +++++++++++++++++------ src/sensorinterface/ADS111xInterface.h | 40 ++++++-- src/sensorinterface/ADS111xPin.cpp | 24 +++++ src/sensorinterface/ADS111xPin.h | 2 +- src/sensors/SensorManager.cpp | 55 ++++++----- 5 files changed, 171 insertions(+), 65 deletions(-) diff --git a/src/sensorinterface/ADS111xInterface.cpp b/src/sensorinterface/ADS111xInterface.cpp index 2a8f65343..e3e2d147f 100644 --- a/src/sensorinterface/ADS111xInterface.cpp +++ b/src/sensorinterface/ADS111xInterface.cpp @@ -1,64 +1,99 @@ +/* + 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, uint8_t address) - : interface{interface} - , address{address} {} +ADS111xInterface::ADS111xInterface( + SensorInterface* interface, + PinInterface* drdy, + uint8_t address +) + : interface { + interface +}, drdy{drdy}, address{address} { +} bool ADS111xInterface::init() { - interface->swapIn(); - Wire.beginTransmission(address); - Wire.write(static_cast(Registers::Addresses::Config)); Registers::Config config{ - .mode = 0b1, // Single-shot - .pga = 0b010, // Gain (FSR): 2.048V - .mux = 0b000, // Doesn't matter - .os = 0b0, // Don't start read .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 }; - uint8_t* bytes = reinterpret_cast(&config); - Wire.write(bytes[0]); - Wire.write(bytes[1]); - auto result = Wire.endTransmission(); - if (result != 0) { + 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) { - interface->swapIn(); - Wire.beginTransmission(address); - Wire.write(static_cast(Registers::Addresses::Config)); + 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 - .compQue = 0b11, // Disable comparator - .compLat = 0b0, // Doesn't matter - .compPol = 0b0, // Doesn't matter - .compMode = 0b0, // Doesn't matter - .dr = 0b111, // 860 samples per second }; - auto* bytes = reinterpret_cast(&config); - Wire.write(bytes[0]); - Wire.write(bytes[1]); - Wire.endTransmission(); + writeRegister(Registers::Addresses::Config, config); - delayMicroseconds(1e6 / 860); + 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(); @@ -69,9 +104,29 @@ float ADS111xInterface::read(uint8_t channel) { uint8_t lsb = Wire.read(); Wire.endTransmission(); - uint16_t value = (msb << 8) | lsb; + return (msb << 8) | lsb; +} + +void ADS111xInterface::registerChannel(uint8_t channel) { + usedPins |= 1 << channel; + if (__builtin_popcount(usedPins) != 1) { + return; + } - return static_cast(value) / maxValue; + // 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 index d1dbabaac..931051e9b 100644 --- a/src/sensorinterface/ADS111xInterface.h +++ b/src/sensorinterface/ADS111xInterface.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + 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 @@ -36,7 +36,7 @@ namespace SlimeVR { class ADS111xInterface { public: - ADS111xInterface(SensorInterface* interface, uint8_t address); + ADS111xInterface(SensorInterface* interface, PinInterface* drdy, uint8_t address); bool init(); float read(uint8_t channel); @@ -46,28 +46,52 @@ class ADS111xInterface { struct Registers { enum class Addresses : uint8_t { - Conversion = 0x00, - Config = 0x01, + Conversion = 0b00, + Config = 0b01, + LoThresh = 0b10, + HiThresh = 0b11, }; struct Config { - uint8_t mode : 1; - uint8_t pga : 3; - uint8_t mux : 3; - uint8_t os : 1; 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 index b12219a71..cc07b549c 100644 --- a/src/sensorinterface/ADS111xPin.cpp +++ b/src/sensorinterface/ADS111xPin.cpp @@ -1,3 +1,25 @@ +/* + 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 @@ -8,6 +30,8 @@ ADS111xPin::ADS111xPin(ADS111xInterface* interface, uint8_t channel) : ads111x{interface} , channel{channel} { assert(channel < 4); + + interface->registerChannel(channel); } int ADS111xPin::digitalRead() { return analogRead() >= 0.5f; } diff --git a/src/sensorinterface/ADS111xPin.h b/src/sensorinterface/ADS111xPin.h index 8e90e7579..be38485de 100644 --- a/src/sensorinterface/ADS111xPin.h +++ b/src/sensorinterface/ADS111xPin.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2024 Gorbit99 & SlimeVR Contributors + 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 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 32352e444..ca422ed3a 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -95,7 +95,8 @@ void SensorManager::setup() { std::map mcpPinInterfaces; std::map, I2CWireSensorInterface*> i2cWireInterfaces; std::map, I2CPCASensorInterface*> pcaWireInterfaces; - std::map, ADS111xInterface*> adsInterfaces; + std::map, ADS111xInterface*> + adsInterfaces; std::map, ADS111xPin*> adsPins; [[maybe_unused]] auto directPin = [&](int pin) { @@ -135,30 +136,32 @@ void SensorManager::setup() { return pcaWireInterfaces[pair]; }; - [[maybe_unused]] auto adsInterface = [&](SensorInterface* interface, int addr) { - auto key = std::make_tuple(interface, addr); - if (!adsInterfaces.contains(key)) { - auto ptr = new ADS111xInterface(interface, addr); - if (!ptr->init()) { - return static_cast(nullptr); - } - adsInterfaces[key] = ptr; - } - return adsInterfaces[key]; - }; - - [[maybe_unused]] auto adsPin = [&](SensorInterface* interface, int addr, int ch) { - auto ads = adsInterface(interface, addr); - if (ads == nullptr) { - return static_cast(nullptr); - } - auto key = std::make_tuple(ads, ch); - if (!adsPins.contains(key)) { - auto ptr = new ADS111xPin(ads, ch); - adsPins[key] = ptr; - } - return adsPins[key]; - }; + [[maybe_unused]] auto adsInterface + = [&](SensorInterface* interface, PinInterface* drdy, int addr) { + auto key = std::make_tuple(interface, drdy, addr); + if (!adsInterfaces.contains(key)) { + auto ptr = new ADS111xInterface(interface, drdy, addr); + if (!ptr->init()) { + return static_cast(nullptr); + } + adsInterfaces[key] = ptr; + } + return adsInterfaces[key]; + }; + + [[maybe_unused]] auto adsPin + = [&](SensorInterface* interface, PinInterface* drdy, int addr, int ch) { + auto ads = adsInterface(interface, drdy, addr); + if (ads == nullptr) { + return static_cast(nullptr); + } + auto key = std::make_tuple(ads, ch); + if (!adsPins.contains(key)) { + auto ptr = new ADS111xPin(ads, ch); + adsPins[key] = ptr; + } + return adsPins[key]; + }; uint8_t sensorID = 0; uint8_t activeSensorCount = 0; if (m_MCP.begin_I2C()) { @@ -170,7 +173,7 @@ void SensorManager::setup() { #define DIRECT_WIRE(scl, sda) directWire(scl, sda) #define MCP_PIN(pin) mcpPin(pin) #define PCA_WIRE(scl, sda, addr, ch) pcaWire(scl, sda, addr, ch) -#define ADS_PIN(interface, addr, ch) adsPin(interface, addr, ch) +#define ADS_PIN(interface, drdy, addr, ch) adsPin(interface, drdy, addr, ch) #define SENSOR_DESC_ENTRY(ImuType, ...) \ { \ From cf63f4e94775596098e6f7fdcfab78f170b29dfe Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Sun, 6 Apr 2025 00:25:02 +0200 Subject: [PATCH 7/9] Formatting --- src/sensorinterface/ADS111xInterface.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/sensorinterface/ADS111xInterface.cpp b/src/sensorinterface/ADS111xInterface.cpp index e3e2d147f..d0ad763c7 100644 --- a/src/sensorinterface/ADS111xInterface.cpp +++ b/src/sensorinterface/ADS111xInterface.cpp @@ -31,10 +31,9 @@ ADS111xInterface::ADS111xInterface( PinInterface* drdy, uint8_t address ) - : interface { - interface -}, drdy{drdy}, address{address} { -} + : interface{interface} + , drdy{drdy} + , address{address} {} bool ADS111xInterface::init() { Registers::Config config{ From 6f86f42c935cf7fc09bf19c02609515085f6c543 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Tue, 22 Apr 2025 18:08:54 +0200 Subject: [PATCH 8/9] Mux working --- src/sensorinterface/ParallelMuxInterface.cpp | 1 + src/sensorinterface/ParallelMuxInterface.h | 1 + src/sensorinterface/ParallelMuxPin.h | 2 ++ src/sensors/ADCResistanceSensor.cpp | 2 ++ src/sensors/ADCResistanceSensor.h | 3 ++- 5 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/sensorinterface/ParallelMuxInterface.cpp b/src/sensorinterface/ParallelMuxInterface.cpp index 7371d05db..24d60ea32 100644 --- a/src/sensorinterface/ParallelMuxInterface.cpp +++ b/src/sensorinterface/ParallelMuxInterface.cpp @@ -95,6 +95,7 @@ void ParallelMuxInterface::switchTo(uint8_t address) { } currentAddress = address; + delay(1); } } // namespace SlimeVR diff --git a/src/sensorinterface/ParallelMuxInterface.h b/src/sensorinterface/ParallelMuxInterface.h index 97843befb..b678d0c46 100644 --- a/src/sensorinterface/ParallelMuxInterface.h +++ b/src/sensorinterface/ParallelMuxInterface.h @@ -22,6 +22,7 @@ #include +#include #include namespace SlimeVR { diff --git a/src/sensorinterface/ParallelMuxPin.h b/src/sensorinterface/ParallelMuxPin.h index 6010957bb..d308f455c 100644 --- a/src/sensorinterface/ParallelMuxPin.h +++ b/src/sensorinterface/ParallelMuxPin.h @@ -20,6 +20,8 @@ #pragma once +#include + #include "PinInterface.h" #include "sensorinterface/ParallelMuxInterface.h" namespace SlimeVR { diff --git a/src/sensors/ADCResistanceSensor.cpp b/src/sensors/ADCResistanceSensor.cpp index 9b0e52b8e..840af04db 100644 --- a/src/sensors/ADCResistanceSensor.cpp +++ b/src/sensors/ADCResistanceSensor.cpp @@ -39,6 +39,8 @@ ADCResistanceSensor::ADCResistanceSensor( lastSampleMicros = micros(); }; +void ADCResistanceSensor::motionSetup() { m_PinInterface->pinMode(INPUT); } + void ADCResistanceSensor::motionLoop() { if (micros() - lastSampleMicros < samplingStepMicros) { return; diff --git a/src/sensors/ADCResistanceSensor.h b/src/sensors/ADCResistanceSensor.h index 6867d55c8..1f3cf9b8f 100644 --- a/src/sensors/ADCResistanceSensor.h +++ b/src/sensors/ADCResistanceSensor.h @@ -39,6 +39,7 @@ class ADCResistanceSensor : public Sensor { ); ~ADCResistanceSensor() = default; + void motionSetup() final; void motionLoop() final; void sendData() final; bool hasNewDataToSend() final; @@ -50,7 +51,7 @@ class ADCResistanceSensor : public Sensor { }; private: - static constexpr uint32_t samplingRateHz = 60; + static constexpr uint32_t samplingRateHz = 120; static constexpr uint64_t samplingStepMicros = 1000'000 / samplingRateHz; PinInterface* m_PinInterface; From ac1e726c8fde8ad3ccbe35ffa310e9e986053069 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Tue, 22 Apr 2025 18:27:49 +0200 Subject: [PATCH 9/9] 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 0701a1c35..2b6ac940f 100644 --- a/src/sensorinterface/SensorInterfaceManager.h +++ b/src/sensorinterface/SensorInterfaceManager.h @@ -100,8 +100,9 @@ class SensorInterfaceManager { inline auto& parallelMuxPinInterface() { return parallelMuxPinInterfaces; } private: - InterfaceCache directPinInterfaces{ - [](int pin) { return pin != 255 && pin != -1; }}; + InterfaceCache directPinInterfaces{[](int pin) { + return pin != 255 && pin != -1; + }}; InterfaceCache mcpPinInterfaces; InterfaceCache i2cWireInterfaces; InterfaceCache pcaWireInterfaces;