From 961efb8de9edd7088cf6473c433fd3eb4fdbbf2c Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Thu, 5 Sep 2024 23:22:39 +0200 Subject: [PATCH] I love C++ <3 --- lib/bno080/BNO080.cpp | 4 +-- lib/bno080/BNO080.h | 12 ++++---- src/sensors/SensorManager.cpp | 56 ++++++++++++++++++++++++++++++++--- src/sensors/SensorManager.h | 4 ++- 4 files changed, 63 insertions(+), 13 deletions(-) diff --git a/lib/bno080/BNO080.cpp b/lib/bno080/BNO080.cpp index 4259d9399..430712466 100644 --- a/lib/bno080/BNO080.cpp +++ b/lib/bno080/BNO080.cpp @@ -43,7 +43,7 @@ //Attempt communication with the device //Return true if we got a 'Polo' back from Marco -boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, std::shared_ptr intPin) +boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, PinInterface* intPin) { _deviceAddress = deviceAddress; //If provided, store the I2C address from user _i2cPort = &wirePort; //Grab which port the user wants us to use @@ -99,7 +99,7 @@ boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, std::shared_ptr< return tBoardInfoReceived; } -boolean BNO080::beginSPI(std::shared_ptr user_CSPin, std::shared_ptr user_WAKPin, std::shared_ptr user_INTPin, std::shared_ptr user_RSTPin, uint32_t spiPortSpeed, SPIClass &spiPort) +boolean BNO080::beginSPI(PinInterface* user_CSPin, PinInterface* user_WAKPin, PinInterface* user_INTPin, PinInterface* user_RSTPin, uint32_t spiPortSpeed, SPIClass &spiPort) { _i2cPort = NULL; //This null tells the send/receive functions to use SPI diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index 3bc814269..7c1339b8f 100644 --- a/lib/bno080/BNO080.h +++ b/lib/bno080/BNO080.h @@ -152,8 +152,8 @@ struct BNO080Error { class BNO080 { public: - boolean begin(uint8_t deviceAddress = BNO080_DEFAULT_ADDRESS, TwoWire &wirePort = Wire, std::shared_ptr intPin = nullptr); //By default use the default I2C addres, and use Wire port, and don't declare an INT pin - boolean beginSPI(std::shared_ptr user_CSPin, std::shared_ptr user_WAKPin, std::shared_ptr user_INTPin, std::shared_ptr user_RSTPin, uint32_t spiPortSpeed = 3000000, SPIClass &spiPort = SPI); + boolean begin(uint8_t deviceAddress = BNO080_DEFAULT_ADDRESS, TwoWire &wirePort = Wire, PinInterface* intPin = nullptr); //By default use the default I2C addres, and use Wire port, and don't declare an INT pin + boolean beginSPI(PinInterface* user_CSPin, PinInterface* user_WAKPin, PinInterface* user_INTPin, PinInterface* user_RSTPin, uint32_t spiPortSpeed = 3000000, SPIClass &spiPort = SPI); void enableDebugging(Stream &debugPort = Serial); //Turn on debug printing. If user doesn't specify then Serial will be used. @@ -313,10 +313,10 @@ class BNO080 SPIClass *_spiPort; //The generic connection to user's chosen SPI hardware unsigned long _spiPortSpeed; //Optional user defined port speed - std::shared_ptr _cs; //Pins needed for SPI - std::shared_ptr _wake; - std::shared_ptr _int; - std::shared_ptr _rst; + PinInterface* _cs; //Pins needed for SPI + PinInterface* _wake; + PinInterface* _int; + PinInterface* _rst; //These are the raw sensor values (without Q applied) pulled from the user requested Input Report uint16_t rawAccelX, rawAccelY, rawAccelZ, accelAccuracy; diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 296576e20..f945de64d 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -43,6 +43,8 @@ #include "sensorinterface/MCP23X17PinInterface.h" #include "sensorinterface/I2CPCAInterface.h" +#include + namespace SlimeVR { namespace Sensors @@ -56,6 +58,52 @@ namespace SlimeVR using SoftFusionMPU6050 = SoftFusionSensor; Adafruit_MCP23X17 mcp; + std::map> directPinInterfaces; + std::map> mcpPinInterfaces; + std::map, std::unique_ptr> i2cWireInterfaces; + std::map, std::unique_ptr> pcaWireInterfaces; + + DirectPinInterface* directPin(int pin) + { + if(!directPinInterfaces.contains(pin)) + { + auto ptr = std::make_unique(pin); + directPinInterfaces[pin] = std::move(ptr); + } + return directPinInterfaces[pin].get(); + } + + MCP23X17PinInterface* mcpPin(int pin) + { + if(!mcpPinInterfaces.contains(pin)) + { + auto ptr = std::make_unique(&mcp, pin); + mcpPinInterfaces[pin] = std::move(ptr); + } + return mcpPinInterfaces[pin].get(); + } + + I2CWireSensorInterface* directWire(int scl, int sda) + { + auto pair = std::make_tuple(scl, sda); + if(!i2cWireInterfaces.contains(pair)) + { + auto ptr = std::make_unique(scl, sda); + i2cWireInterfaces[pair] = std::move(ptr); + } + return i2cWireInterfaces[pair].get(); + } + + I2CPCASensorInterface* pcaWire(int scl, int sda, int addr, int ch) + { + auto pair = std::make_tuple(scl, sda, addr, ch); + if(!pcaWireInterfaces.contains(pair)) + { + auto ptr = std::make_unique(scl, sda, addr, ch); + pcaWireInterfaces[pair] = std::move(ptr); + } + return pcaWireInterfaces[pair].get(); + } void SensorManager::setup() { @@ -65,10 +113,10 @@ namespace SlimeVR mcp.begin_I2C(); #define NO_PIN nullptr -#define DIRECT_PIN(pin) std::make_shared(pin) -#define DIRECT_WIRE(scl, sda) std::make_shared(sda, scl) -#define MCP_PIN(pin) std::make_shared(&mcp, pin) -#define PCA_WIRE(scl, sda, addr, ch) std::make_shared(scl, sda, addr, ch) +#define DIRECT_PIN(pin) directPin(pin) +#define DIRECT_WIRE(scl, sda) directWire(scl, sda) +#define MCP_PIN(pin) mcpPin(pin) +#define PCA_WIRE(scl, sda, addr, ch) pcaWire(scl, sda, addr, ch) #define IMU_DESC_ENTRY(ImuType, ...) \ { \ diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index ea09887b7..5c547f31f 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -63,7 +63,9 @@ namespace SlimeVR std::vector> m_Sensors; template - std::unique_ptr buildSensor(uint8_t sensorID, uint8_t addrSuppl, float rotation, std::shared_ptr sensorInterface, bool optional = false, std::shared_ptr intPin = nullptr, int extraParam = 0) + std::unique_ptr buildSensor( + uint8_t sensorID, uint8_t addrSuppl, float rotation, SensorInterface* sensorInterface, + bool optional = false, PinInterface* intPin = nullptr, int extraParam = 0) { const uint8_t address = ImuType::Address + addrSuppl; m_Logger.trace("Building IMU with: id=%d,\n\