diff --git a/src/Main/Sensors/Sensors.cpp b/src/Main/Sensors/Sensors.cpp
index d8dd731aa2059794cd9e3a55f5c4c4aacae34eb5..9a86955e5523e2b57e6bc00c0df88721c442c0b7 100644
--- a/src/Main/Sensors/Sensors.cpp
+++ b/src/Main/Sensors/Sensors.cpp
@@ -625,8 +625,10 @@ void Sensors::lsm6dsrxCallback()
         return;
 
     // For every instance inside the fifo log the sample
-    for (uint16_t i = 0; i < lsm6dsrx->getLastFifoSize(); i++)
-        sdLogger.log(lsm6dsrx->getLastFifo().at(i));
+    uint16_t lastFifoSize;
+    const auto lastFifo = lsm6dsrx->getLastFifo(lastFifoSize);
+    for (uint16_t i = 0; i < lastFifoSize; i++)
+        sdLogger.log(lastFifo.at(i));
 }
 
 void Sensors::vn100Init()
diff --git a/src/Motor/Sensors/Sensors.cpp b/src/Motor/Sensors/Sensors.cpp
index 5d8ed74e473e1d8f42bc734e20e52b1f65541db7..c930178caefaa881ba8822aeb866d65913b0601e 100644
--- a/src/Motor/Sensors/Sensors.cpp
+++ b/src/Motor/Sensors/Sensors.cpp
@@ -259,9 +259,12 @@ void Sensors::lsm6dsrxCallback()
     if (!lsm6dsrx)
         return;
 
+    uint16_t lastFifoSize;
+    const auto lastFifo = lsm6dsrx->getLastFifo(lastFifoSize);
+
     // For every instance inside the fifo log the sample
-    for (uint16_t i = 0; i < lsm6dsrx->getLastFifoSize(); i++)
-        sdLogger.log(lsm6dsrx->getLastFifo().at(i));
+    for (uint16_t i = 0; i < lastFifoSize; i++)
+        sdLogger.log(lastFifo.at(i));
 }
 
 void Sensors::ads131m08Init()
diff --git a/src/Payload/Sensors/Sensors.cpp b/src/Payload/Sensors/Sensors.cpp
index c763eaddb25b73be5078c7454b9d0bf21facdf49..0adc003da1e30073decc3ddf41883b59d9e07962 100644
--- a/src/Payload/Sensors/Sensors.cpp
+++ b/src/Payload/Sensors/Sensors.cpp
@@ -541,13 +541,13 @@ void Sensors::lsm6dsrxCallback()
         return;
 
     // We can skip logging the last sample since we are logging the fifo
-    auto& logger  = Logger::getInstance();
-    auto& fifo    = lsm6dsrx->getLastFifo();
-    auto fifoSize = lsm6dsrx->getLastFifoSize();
+    auto& logger = Logger::getInstance();
+    uint16_t lastFifoSize;
+    const auto lastFifo = lsm6dsrx->getLastFifo(lastFifoSize);
 
     // For every instance inside the fifo log the sample
-    for (auto i = 0; i < fifoSize; i++)
-        logger.log(fifo.at(i));
+    for (uint16_t i = 0; i < lastFifoSize; i++)
+        logger.log(lastFifo.at(i));
 }
 
 void Sensors::ads131m08Init()