diff --git a/esp32/include/peripherals/barometer.h b/esp32/include/peripherals/barometer.h
index 5a67232..ae375e0 100644
--- a/esp32/include/peripherals/barometer.h
+++ b/esp32/include/peripherals/barometer.h
@@ -38,7 +38,7 @@ struct BarometerMsg : public SensorMessageBase {
 
 class Barometer : public SensorBase<BarometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _) override {
         _msg.success = _bmp.begin();
         return _msg.success;
     }
diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index a872e9e..c88142a 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -48,7 +48,7 @@ struct IMUAnglesMsg : public SensorMessageBase {
 
 class IMU : public SensorBase<IMUAnglesMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _arg = nullptr) override {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         _msg.success = _imu.testConnection();
@@ -88,23 +88,25 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #endif
 #if FT_ENABLED(USE_ICM20948)
     #if USE_ICM20948_SPIMODE > 0
-        _imu.begin(CS_PIN, SPI_PORT);
+        _imu = (ICM_20948_SPI*)_arg;
+        _imu->begin(CS_PIN, SPI_PORT);
     #else
-        _imu.begin(Wire, 1, 0xFF);
+        _imu = (ICM_20948_I2C*)_arg;
+        _imu->begin(Wire, 1, 0xFF);
     #endif
-    if (_imu.status != ICM_20948_Stat_Ok){ return false; }
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
     
-    _imu.setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
-    if (_imu.status != ICM_20948_Stat_Ok){ return false; }
+    _imu->setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
     
     ICM_20948_fss_t myFSS;
     myFSS.a = gpm2;
     myFSS.g = dps250;
-    _imu.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
-    if (_imu.status != ICM_20948_Stat_Ok){ return false; }
+    _imu->setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
     // TODO: Setup low pass filter config
-    _imu.startupMagnetometer();
-    if (_imu.status != ICM_20948_Stat_Ok){ return false; }
+    _imu->startupMagnetometer();
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
 #endif
         return _msg.success;
     }
@@ -129,12 +131,12 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         return false;
 #endif
 #if FT_ENABLED(USE_ICM20948)
-        if (_imu.dataReady())
+        if (_imu->dataReady())
         {   
-            _imu.getAGMT();
-            _msg.rpy[0] = _imu.gyrX();
-            _msg.rpy[1] = _imu.gyrY();
-            _msg.rpy[2] = _imu.gyrZ();
+            _imu->getAGMT();
+            _msg.rpy[0] = _imu->gyrX();
+            _msg.rpy[1] = _imu->gyrY();
+            _msg.rpy[2] = _imu->gyrZ();
         }
 #endif
 #if FT_ENABLED(USE_BNO055)
@@ -180,10 +182,10 @@ class IMU : public SensorBase<IMUAnglesMsg> {
     #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
         #define SPI_PORT SPI // TODO in periphearals_seetings.h
         #define CS_PIN 2 
-        ICM_20948_SPI _imu;
+        ICM_20948_SPI* _imu;
     #else
         //#define WIRE_PORT Wire 
-        ICM_20948_I2C _imu;
+        ICM_20948_I2C* _imu;
     #endif
 #endif
 };
\ No newline at end of file
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index c9ca221..0ab2c43 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -38,7 +38,7 @@ struct MagnetometerMsg : public SensorMessageBase {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _) override {
         _msg.success = _mag.begin();
         return _msg.success;
     }
diff --git a/esp32/include/peripherals/sensor.hpp b/esp32/include/peripherals/sensor.hpp
index a9ec8c6..85d7b93 100644
--- a/esp32/include/peripherals/sensor.hpp
+++ b/esp32/include/peripherals/sensor.hpp
@@ -17,7 +17,7 @@ class SensorBase {
   public:
     SensorBase() {}
 
-    virtual bool initialize() = 0;
+    virtual bool initialize(void* _arg) = 0;
 
     virtual bool update() = 0;
 
diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp
index 6ba7cea..3619380 100644
--- a/esp32/src/peripherals/peripherals.cpp
+++ b/esp32/src/peripherals/peripherals.cpp
@@ -15,8 +15,15 @@ void Peripherals::begin() {
 
     updatePins();
 
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
     if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
+#elif FT_ENABLED(USE_ICM20948)
+    #if USE_ICM20948_SPIMODE > 0
+        ICM_20948_SPI* icm20948 = new ICM_20948_SPI;
+    #else
+        ICM_20948_I2C* icm20948 = new ICM_20948_I2C;
+    #endif
+    if (!_imu.initialize(icm20948)) ESP_LOGE("IMUService", "IMU initialize failed (ICM20948)");
 #endif
 #if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
     if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");