diff --git a/esp32/include/peripherals/barometer.h b/esp32/include/peripherals/barometer.h
index 75088d8..6b348f1 100644
--- a/esp32/include/peripherals/barometer.h
+++ b/esp32/include/peripherals/barometer.h
@@ -13,7 +13,7 @@ struct BarometerMsg {
 
 class Barometer : public SensorBase<BarometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _) override {
         _msg.success = _bmp.begin();
         if (_msg.success) {
             ESP_LOGI("BMP", "BMP180 initialized successfully");
diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index a40250e..2243098 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -24,7 +24,7 @@ struct IMUAnglesMsg {
 
 class IMU : public SensorBase<IMUAnglesMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _arg = nullptr) override {
 #if FT_ENABLED(USE_MPU6050)
         _msg.success = _imu.begin();
         if (!_msg.success) {
@@ -42,24 +42,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; }
-        ESP_LOGI("IMU", "BNO055 initialized successfully");
+    _imu->startupMagnetometer();
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
 #endif
         return _msg.success;
     }
@@ -74,12 +75,12 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         _msg.temperature = _imu.getTemperature();
 #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)
@@ -135,10 +136,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
 };
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index 9a830ed..2ab0a10 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -12,7 +12,7 @@ struct MagnetometerMsg {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _) override {
         _msg.success = _mag.begin();
         if (_msg.success) {
             ESP_LOGI("MAG", "HMC5883L initialized successfully");
diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp
index ccdaa25..5f7351a 100644
--- a/esp32/src/peripherals/peripherals.cpp
+++ b/esp32/src/peripherals/peripherals.cpp
@@ -16,8 +16,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");