diff --git a/workspace/TS100/Core/Inc/LIS2DH12.hpp b/workspace/TS100/Core/Inc/LIS2DH12.hpp
index 21a7bbc4..5a30288f 100644
--- a/workspace/TS100/Core/Inc/LIS2DH12.hpp
+++ b/workspace/TS100/Core/Inc/LIS2DH12.hpp
@@ -34,7 +34,7 @@ public:
 		return static_cast<Orientation>((FRToSI2C::I2C_RegisterRead(LIS2DH_I2C_ADDRESS,LIS_INT2_SRC) >> 2) - 1);
 #endif
 	}
-	static void getAxisReadings(int16_t *x, int16_t *y, int16_t *z);
+	static void getAxisReadings(int16_t& x, int16_t& y, int16_t& z);
 
 private:
 };
diff --git a/workspace/TS100/Core/Inc/MMA8652FC.hpp b/workspace/TS100/Core/Inc/MMA8652FC.hpp
index 7bf1cafa..b6d2938e 100644
--- a/workspace/TS100/Core/Inc/MMA8652FC.hpp
+++ b/workspace/TS100/Core/Inc/MMA8652FC.hpp
@@ -19,7 +19,7 @@ public:
 
 	static void initalize();		// Initalize the system
 	static Orientation getOrientation();// Reads the I2C register and returns the orientation (true == left)
-	static void getAxisReadings(int16_t *x, int16_t *y, int16_t *z);
+	static void getAxisReadings(int16_t& x, int16_t& y, int16_t& z);
 
 private:
 };
diff --git a/workspace/TS100/Core/Src/LIS2DH12.cpp b/workspace/TS100/Core/Src/LIS2DH12.cpp
index 544ead40..b1d25703 100644
--- a/workspace/TS100/Core/Src/LIS2DH12.cpp
+++ b/workspace/TS100/Core/Src/LIS2DH12.cpp
@@ -5,9 +5,10 @@
  *      Author: Ralim
  */
 
-#include <LIS2DH12.hpp>
-#include "cmsis_os.h"
+#include <array>
 
+#include "LIS2DH12.hpp"
+#include "cmsis_os.h"
 
 typedef struct {
         const uint8_t reg;
@@ -36,14 +37,14 @@ void LIS2DH12::initalize() {
     }
 }
 
-void LIS2DH12::getAxisReadings(int16_t* x, int16_t* y, int16_t* z) {
-	uint8_t tempArr[6];
-FRToSI2C::Mem_Read(LIS2DH_I2C_ADDRESS, 0xA8, I2C_MEMADD_SIZE_8BIT,
-			(uint8_t*) tempArr, 6);
+void LIS2DH12::getAxisReadings(int16_t& x, int16_t& y, int16_t& z) {
+    std::array<int16_t, 3> sensorData;
 
-	(*x) = ((uint16_t) (tempArr[1] << 8 | tempArr[0]));
-	(*y) = ((uint16_t) (tempArr[3] << 8 | tempArr[2]));
-	(*z) = ((uint16_t) (tempArr[5] << 8 | tempArr[4]));
+    FRToSI2C::Mem_Read(LIS2DH_I2C_ADDRESS, 0xA8, I2C_MEMADD_SIZE_8BIT,
+        reinterpret_cast<uint8_t*>(sensorData.begin()),
+        sensorData.size() * sizeof(int16_t));
+
+    x = sensorData[0];
+    y = sensorData[1];
+    z = sensorData[2];
 }
-
-
diff --git a/workspace/TS100/Core/Src/MMA8652FC.cpp b/workspace/TS100/Core/Src/MMA8652FC.cpp
index a6757604..ebe10568 100644
--- a/workspace/TS100/Core/Src/MMA8652FC.cpp
+++ b/workspace/TS100/Core/Src/MMA8652FC.cpp
@@ -5,7 +5,9 @@
  *      Author: Ben V. Brown
  */
 
-#include <MMA8652FC.hpp>
+#include <array>
+
+#include "MMA8652FC.hpp"
 #include "cmsis_os.h"
 
 typedef struct {
@@ -62,12 +64,15 @@ Orientation MMA8652FC::getOrientation() {
 
 	return ORIENTATION_FLAT;
 }
-void MMA8652FC::getAxisReadings(int16_t *x, int16_t *y, int16_t *z) {
-	uint8_t tempArr[6];
-	FRToSI2C::Mem_Read( MMA8652FC_I2C_ADDRESS, OUT_X_MSB_REG, I2C_MEMADD_SIZE_8BIT,
-			(uint8_t*) tempArr, 6);
 
-	(*x) = tempArr[0] << 8 | tempArr[1];
-	(*y) = tempArr[2] << 8 | tempArr[3];
-	(*z) = tempArr[4] << 8 | tempArr[5];
+void MMA8652FC::getAxisReadings(int16_t& x, int16_t& y, int16_t& z) {
+	std::array<int16_t, 3> sensorData;
+
+	FRToSI2C::Mem_Read(MMA8652FC_I2C_ADDRESS, OUT_X_MSB_REG, I2C_MEMADD_SIZE_8BIT,
+		reinterpret_cast<uint8_t*>(sensorData.begin()),
+		sensorData.size() * sizeof(int16_t));
+
+	x = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(&sensorData[0])));
+	y = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(&sensorData[1])));
+	z = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(&sensorData[2])));
 }
diff --git a/workspace/TS100/Core/Src/main.cpp b/workspace/TS100/Core/Src/main.cpp
index f3994694..c3eb9076 100644
--- a/workspace/TS100/Core/Src/main.cpp
+++ b/workspace/TS100/Core/Src/main.cpp
@@ -254,10 +254,10 @@ void startMOVTask(void const *argument __unused) {
 		threshold -= systemSettings.sensitivity * 200;  // 200 is the step size
 
 		if (PCBVersion == 2) {
-			LIS2DH12::getAxisReadings(&tx, &ty, &tz);
+			LIS2DH12::getAxisReadings(tx, ty, tz);
 			rotation = LIS2DH12::getOrientation();
 		} else if (PCBVersion == 1) {
-			MMA8652FC::getAxisReadings(&tx, &ty, &tz);
+			MMA8652FC::getAxisReadings(tx, ty, tz);
 			rotation = MMA8652FC::getOrientation();
 		}
 		if (systemSettings.OrientationMode == 2) {