diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index 2243098..d43746d 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -43,10 +43,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; }
     
@@ -59,8 +60,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 2ab0a10..def5589 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -12,23 +12,48 @@ struct MagnetometerMsg {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize(void* _) override {
-        _msg.success = _mag.begin();
-        if (_msg.success) {
-            ESP_LOGI("MAG", "HMC5883L initialized successfully");
-        } else {
-            ESP_LOGE("MAG", "HMC5883L initialization failed");
-        }
+    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;
-        if (!_mag.update()) return false;
-        _msg.rpy[0] = _mag.getMagX();
-        _msg.rpy[1] = _mag.getMagY();
-        _msg.rpy[2] = _mag.getMagZ();
-        _msg.heading = _mag.getHeading();
+        #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;
+        _msg.heading *= 180 / M_PI;
         return true;
     }
 
@@ -38,5 +63,18 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
     float getHeading() { return _msg.heading; }
 
   private:
-    HMC5883LDriver _mag;
+    
+    #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;
 };
diff --git a/esp32/include/peripherals/peripherals.h b/esp32/include/peripherals/peripherals.h
index 0f9b7e0..d1c0c6b 100644
--- a/esp32/include/peripherals/peripherals.h
+++ b/esp32/include/peripherals/peripherals.h
@@ -77,7 +77,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 5f7351a..8dc53ff 100644
--- a/esp32/src/peripherals/peripherals.cpp
+++ b/esp32/src/peripherals/peripherals.cpp
@@ -16,25 +16,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);
@@ -118,7 +133,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();
@@ -194,7 +209,7 @@ float Peripherals::leftDistance() { return _left_distance; }
 float Peripherals::rightDistance() { return _right_distance; }
 
 bool Peripherals::calibrateIMU() {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     beginTransaction();
     bool result = _imu.calibrate();
     endTransaction();