I love C++ <3

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<PinInterface> 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<PinInterface> user_CSPin, std::shared_ptr<PinInterface> user_WAKPin, std::shared_ptr<PinInterface> user_INTPin, std::shared_ptr<PinInterface> 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
 

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<PinInterface> 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<PinInterface> user_CSPin, std::shared_ptr<PinInterface> user_WAKPin, std::shared_ptr<PinInterface> user_INTPin, std::shared_ptr<PinInterface> 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<PinInterface> _cs;				 //Pins needed for SPI
-	std::shared_ptr<PinInterface> _wake;
-	std::shared_ptr<PinInterface> _int;
-	std::shared_ptr<PinInterface> _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;

src/sensors/SensorManager.cpp

@@ -43,6 +43,8 @@
 #include "sensorinterface/MCP23X17PinInterface.h"
 #include "sensorinterface/I2CPCAInterface.h"
 
+#include <map>
+
 namespace SlimeVR
 {
     namespace Sensors
@@ -56,6 +58,52 @@ namespace SlimeVR
         using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
 
 		Adafruit_MCP23X17 mcp;
+		std::map<int, std::unique_ptr<DirectPinInterface>> directPinInterfaces;
+		std::map<int, std::unique_ptr<MCP23X17PinInterface>> mcpPinInterfaces;
+		std::map<std::tuple<int, int>, std::unique_ptr<I2CWireSensorInterface>> i2cWireInterfaces;
+		std::map<std::tuple<int, int, int, int>, std::unique_ptr<I2CPCASensorInterface>> pcaWireInterfaces;
+
+		DirectPinInterface* directPin(int pin)
+		{
+			if(!directPinInterfaces.contains(pin))
+			{
+				auto ptr = std::make_unique<DirectPinInterface>(pin);
+				directPinInterfaces[pin] = std::move(ptr);
+			}
+			return directPinInterfaces[pin].get();
+		}
+
+		MCP23X17PinInterface* mcpPin(int pin)
+		{
+			if(!mcpPinInterfaces.contains(pin))
+			{
+				auto ptr = std::make_unique<MCP23X17PinInterface>(&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<I2CWireSensorInterface>(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<I2CPCASensorInterface>(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<DirectPinInterface>(pin)
-#define DIRECT_WIRE(scl, sda) std::make_shared<I2CWireSensorInterface>(sda, scl)
-#define MCP_PIN(pin) std::make_shared<MCP23X17PinInterface>(&mcp, pin)
-#define PCA_WIRE(scl, sda, addr, ch) std::make_shared<I2CPCASensorInterface>(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, ...)                                  \
             {                                                         \

src/sensors/SensorManager.h

@@ -63,7 +63,9 @@ namespace SlimeVR
             std::vector<std::unique_ptr<Sensor>> m_Sensors;
 
             template <typename ImuType>
-            std::unique_ptr<Sensor> buildSensor(uint8_t sensorID, uint8_t addrSuppl, float rotation, std::shared_ptr<SensorInterface> sensorInterface, bool optional = false, std::shared_ptr<PinInterface> intPin = nullptr, int extraParam = 0)
+            std::unique_ptr<Sensor> 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\