diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0e91c7441f83e25e0934fb95f11326ecaf0d4fc5..2e28274d6c43e8bb72a37d6435f9592bb23fd2f3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,8 +21,6 @@
 
 cmake_minimum_required(VERSION 3.16)
 enable_testing()
-include(skyward-boardcore/cmake/sbs.cmake)
-include(cmake/dependencies.cmake)
 
 # -----------------------------------------------------------------------------#
 #                                    Project                                   #
@@ -30,6 +28,9 @@ include(cmake/dependencies.cmake)
 
 project(OnBoardSoftware)
 
+include(skyward-boardcore/cmake/sbs.cmake)
+include(cmake/dependencies.cmake)
+
 #-----------------------------------------------------------------------------#
 #                              Flight entrypoints                             #
 #-----------------------------------------------------------------------------#
diff --git a/skyward-boardcore b/skyward-boardcore
index 93254875ba79c70c4e1b4fb1b144a8b117337abf..ef4e55b6575914cc021c4eb36173db9339d3a7c8 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit 93254875ba79c70c4e1b4fb1b144a8b117337abf
+Subproject commit ef4e55b6575914cc021c4eb36173db9339d3a7c8
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()