diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index c88142a..cdd5731 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -89,10 +89,11 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_ICM20948)
     #if USE_ICM20948_SPIMODE > 0
         _imu = (ICM_20948_SPI*)_arg;
-        _imu->begin(CS_PIN, SPI_PORT);
+        if (true || !_imu->isConnected()) { _imu->begin(CS_PIN, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode"); }
     #else
         _imu = (ICM_20948_I2C*)_arg;
-        _imu->begin(Wire, 1, 0xFF);
+        if (true || !_imu->isConnected()) { _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode"); }
+        
     #endif
     if (_imu->status != ICM_20948_Stat_Ok){ return false; }
     
@@ -105,8 +106,6 @@ class IMU : public SensorBase<IMUAnglesMsg> {
     _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; }
 #endif
         return _msg.success;
     }
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index 0ab2c43..27bf046 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -11,6 +11,7 @@
 
 #include <peripherals/sensor.hpp>
 
+
 struct MagnetometerMsg : public SensorMessageBase {
     float rpy[3] {0, 0, 0};
     float heading {-1};
@@ -38,20 +39,44 @@ struct MagnetometerMsg : public SensorMessageBase {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize(void* _) override {
-        _msg.success = _mag.begin();
+    bool initialize(void* _arg) override {
+        #if FT_ENABLED(USE_ICM20948)
+            #if USE_ICM20948_SPIMODE > 0
+                _mag = (ICM_20948_SPI*)_arg;
+                if (true || !_mag->isConnected()) { _mag->begin(CS_PIN, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode"); }
+            #else
+                _mag = (ICM_20948_I2C*)_arg;
+                if (true || !_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
+                
+            #endif
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            
+            _mag->startupMagnetometer();
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+        #elif FT_ENABLED(USE_HMC5883)
+            _msg.success = _mag.begin();
+        #endif
         return _msg.success;
     }
 
     bool update() override {
         if (!_msg.success) return false;
-        sensors_event_t event;
-        bool updated = _mag.getEvent(&event);
-        if (!updated) return false;
-        _msg.rpy[0] = event.magnetic.x;
-        _msg.rpy[1] = event.magnetic.y;
-        _msg.rpy[2] = event.magnetic.z;
-        _msg.heading = atan2(event.magnetic.y, event.magnetic.x);
+        #if FT_ENABLED(USE_ICM20948)
+            _mag->getAGMT();
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            _msg.rpy[0] = _mag->magX();
+            _msg.rpy[1] = _mag->magY();
+            _msg.rpy[2] = _mag->magZ();
+
+        #elif FT_ENABLED(USE_HMC5883)
+            sensors_event_t event;
+            bool updated = _mag.getEvent(&event);
+            if (!updated) return false;
+            _msg.rpy[0] = event.magnetic.x;
+            _msg.rpy[1] = event.magnetic.y;
+            _msg.rpy[2] = event.magnetic.z;
+        #endif
+        _msg.heading = atan2(_msg.rpy[1], _msg.rpy[0]); // atan2(y, x)
         _msg.heading += declinationAngle;
         if (_msg.heading < 0) _msg.heading += 2 * PI;
         if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
@@ -68,6 +93,18 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
     float getHeading() { return _msg.heading; }
 
   private:
-    Adafruit_HMC5883_Unified _mag {12345};
+    
+    #if FT_ENABLED(USE_ICM20948)
+        #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+            #define SPI_PORT SPI // TODO in periphearals_seetings.h
+            #define CS_PIN 2 
+            ICM_20948_SPI* _mag;
+        #else
+            //#define WIRE_PORT Wire 
+            ICM_20948_I2C* _mag;
+        #endif
+    #elif FT_ENABLED(USE_HMC5883)
+        Adafruit_HMC5883_Unified _mag {12345};
+    #endif
     const float declinationAngle = 0.22;
 };
\ No newline at end of file
diff --git a/esp32/include/peripherals/peripherals.h b/esp32/include/peripherals/peripherals.h
index 369e765..3b18b4c 100644
--- a/esp32/include/peripherals/peripherals.h
+++ b/esp32/include/peripherals/peripherals.h
@@ -90,7 +90,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     IMU _imu;
 #endif
-#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
+#if FT_ENABLED(USE_HMC5883) || FT_ENABLED(USE_ICM20948)
     Magnetometer _mag;
 #endif
 #if FT_ENABLED(USE_BMP180)
diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp
index 3619380..1b5b5c6 100644
--- a/esp32/src/peripherals/peripherals.cpp
+++ b/esp32/src/peripherals/peripherals.cpp
@@ -15,25 +15,40 @@ void Peripherals::begin() {
 
     updatePins();
 
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-    if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
-#elif FT_ENABLED(USE_ICM20948)
+#if 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");
+
+// --- IMU ---
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    if (!_imu.initialize(nullptr)) ESP_LOGE("Peripherals", "IMU initialize failed");
+#elif FT_ENABLED(USE_ICM20948)
+    if (!_imu.initialize(icm20948)) ESP_LOGE("Peripherals", "IMU initialize failed (ICM20948)");
 #endif
+
+// --- MAGNETOMETER ---
+#if FT_ENABLED(USE_HMC5883)
+    if (!_mag.initialize(nullptr)) ESP_LOGE("Peripherals", "MAG initialize failed");
+#elif FT_ENABLED(USE_ICM20948)
+    if (!_mag.initialize(icm20948)) ESP_LOGE("Peripherals", "MAG initialize failed (ICM20948)");
+#endif
+
+// --- BMP ---
 #if FT_ENABLED(USE_BMP180)
-    if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
+    if (!_bmp.initialize(nullptr)) ESP_LOGE("Peripherals", "BMP initialize failed");
 #endif
+
+
+// --- GESTURE ---
 #if FT_ENABLED(USE_PAJ7620U2)
-    if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed");
+    if (!_gesture.initialize(nullptr)) ESP_LOGE("Peripherals", "Gesture initialize failed");
 #endif
+
+// --- SONAR ---
 #if FT_ENABLED(USE_USS)
     _left_sonar = std::make_unique<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
     _right_sonar = std::make_unique<NewPing>(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
@@ -96,7 +111,7 @@ bool Peripherals::readImu() {
 
 bool Peripherals::readMag() {
     bool updated = false;
-#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
+#if FT_ENABLED(USE_HMC5883 || USE_ICM20948)
     beginTransaction();
     updated = _mag.update();
     endTransaction();
@@ -176,7 +191,7 @@ void Peripherals::getIMUResult(JsonVariant &root) {
     JsonVariant imu = root["imu"].to<JsonVariant>();
     _imu.getResults(imu);
 #endif
-#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
+#if FT_ENABLED(USE_HMC5883 || USE_ICM20948) // TODO:
     JsonVariant mag = root["mag"].to<JsonVariant>();
     _mag.getResults(mag);
 #endif