diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index 265e6b8..7893a13 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -142,8 +142,10 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
         #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
         if (_imu->dataReady())
-        { 
+        {
             _imu->getAGMT();
+        } else {
+            return false;
         }
         #endif
         _msg.rpy[0] = _imu->accX();
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index 3b7a189..711fb2d 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -75,6 +75,8 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
         if (_imu->dataReady())
         { 
             _imu->getAGMT();
+        } else {
+            return false;
         }
         #endif
         _msg.rpy[0] = _mag->magX();
diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp
index 917c0b7..e16b0ba 100644
--- a/esp32/src/peripherals/peripherals.cpp
+++ b/esp32/src/peripherals/peripherals.cpp
@@ -56,8 +56,14 @@ void Peripherals::begin() {
 };
 
 void Peripherals::update() {
-    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_imu, readImu());
-    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, readMag());
+    bool res = true;
+    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_imu, res = readImu());
+    #ifdef FT_ENABLED(USE_ICM20948)
+        // IF ICM_20948 fails to get IMU, it means that mag also does not have new data
+        CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, if (res) { res = readMag(); } );
+    #else
+        CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, res = readMag());
+    #endif
     CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_gesture, EXECUTE_EVERY_N_MS(100, { readGesture(); }) );
     CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_bmp, EXECUTE_EVERY_N_MS(500, { readBMP(); }) );
     CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_sonar, EXECUTE_EVERY_N_MS(500, { readSonar(); }) );