diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h
index d58773d..9435f15 100644
--- a/Core/Inc/msb.h
+++ b/Core/Inc/msb.h
@@ -23,12 +23,6 @@ int8_t msb_init();
 int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity);
 #endif
 
-#ifdef SENSOR_IMU
-int8_t accel_read(uint16_t accel[3]);
-
-int8_t gyro_read(uint16_t gyro[3]);
-#endif
-
 #ifdef SENSOR_TOF
 int8_t distance_read(int32_t *range_mm);
 #endif
@@ -39,6 +33,9 @@ int8_t debug2_write(bool status);
 
 int8_t vcc5_en_write(bool status);
 
+int32_t imu_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx,
+		     lsm6dso_md_t *imu_md_temp, lsm6dso_data_t *imu_data_temp);
+
 #ifdef SENSOR_SHOCKPOT
 void shockpot_read(uint32_t shockpot_sense);
 #endif
diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index 1098260..c9e7356 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -88,13 +88,13 @@ void vTempMonitor(void *pv_params)
 osThreadId_t imu_monitor_handle;
 const osThreadAttr_t imu_monitor_attributes = {
 	.name = "IMUMonitor",
-	.stack_size = 64 * 8,
+	.stack_size = 128 * 8,
 	.priority = (osPriority_t)osPriorityHigh,
 };
 
 void vIMUMonitor(void *pv_params)
 {
-	//const uint8_t num_samples = 10;
+	// const uint8_t num_samples = 10;
 	can_msg_t imu_accel_msg = { .id = convert_can(CANID_IMU_ACCEL,
 						      device_loc),
 				    .len = 6,
@@ -105,44 +105,56 @@ void vIMUMonitor(void *pv_params)
 				   .data = { 0 } };
 
 	struct __attribute__((__packed__)) {
-		uint16_t accel_x;
-		uint16_t accel_y;
-		uint16_t accel_z;
+		int16_t accel_x;
+		int16_t accel_y;
+		int16_t accel_z;
 	} accel_data;
 
 	struct __attribute__((__packed__)) {
-		uint16_t gyro_x;
-		uint16_t gyro_y;
-		uint16_t gyro_z;
+		int16_t gyro_x;
+		int16_t gyro_y;
+		int16_t gyro_z;
 	} gyro_data;
 
-	uint16_t accel_data_temp[3] = { 0 };
-	uint16_t gyro_data_temp[3] = { 0 };
+	struct __attribute__((__packed__)) {
+		float_t temp;
+	} temperature_data;
 
-	for (;;) {
-		/* Take measurement */
+	stmdev_ctx_t ctx;
+	stmdev_ctx_t aux_ctx;
+	// int16_t temperature_data_temp;
 
-		if (accel_read(accel_data_temp)) {
-			printf("Failed to get IMU acceleration\r\n");
-		}
+	lsm6dso_md_t imu_md_temp;
+	lsm6dso_data_t imu_data_temp;
+
+	/* Add parameters for formatting data */
+	imu_md_temp.ui.gy.fs = LSM6DSO_500dps;
+	imu_md_temp.ui.gy.odr = LSM6DSO_GY_UI_52Hz_LP;
+	imu_md_temp.ui.xl.fs = LSM6DSO_XL_UI_2g;
+	imu_md_temp.ui.xl.odr = LSM6DSO_XL_UI_52Hz_LP;
 
-		if (gyro_read(gyro_data_temp)) {
-			printf("Failed to get IMU gyroscope\r\n");
+	for (;;) {
+		/* Take measurement */
+		if (imu_data_get(&ctx, &aux_ctx, &imu_md_temp,
+				 &imu_data_temp)) {
+			printf("Failed to get IMU data \r\n");
 		}
 
 		/* Run values through LPF of sample size  */
-		accel_data.accel_x = (accel_data.accel_x + accel_data_temp[0]);
-		accel_data.accel_y = (accel_data.accel_y + accel_data_temp[1]);
-		accel_data.accel_z = (accel_data.accel_z + accel_data_temp[2]);
-		gyro_data.gyro_x = (gyro_data.gyro_x + gyro_data_temp[0]);
-		gyro_data.gyro_y = (gyro_data.gyro_y + gyro_data_temp[1]);
-		gyro_data.gyro_z = (gyro_data.gyro_z + gyro_data_temp[2]);
+		accel_data.accel_x = imu_data_temp.ui.xl.mg[0];
+		accel_data.accel_y = imu_data_temp.ui.xl.mg[1];
+		accel_data.accel_z = imu_data_temp.ui.xl.mg[2];
+		gyro_data.gyro_x = imu_data_temp.ui.gy.mdps[0];
+		gyro_data.gyro_y = imu_data_temp.ui.gy.mdps[1];
+		gyro_data.gyro_z = imu_data_temp.ui.gy.mdps[2];
+		temperature_data.temp = imu_data_temp.ui.heat.deg_c;
 
 #ifdef LOG_VERBOSE
 		printf("IMU Accel x: %d y: %d z: %d \r\n", accel_data.accel_x,
 		       accel_data.accel_y, accel_data.accel_z);
 		printf("IMU Gyro x: %d y: %d z: %d \r\n", gyro_data.gyro_x,
 		       gyro_data.gyro_y, gyro_data.gyro_z);
+		printf("IMU Temp: %3.2f °C \r\n", temperature_data.temp);
 #endif
 
 		/* convert to big endian */
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 9c7be3f..3470f5e 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -1,10 +1,11 @@
 #include "msb.h"
-#include "main.h"
 #include "lsm6dso.h"
+#include "lsm6dso_reg.h"
+#include "main.h"
 #include <assert.h>
+#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <stdio.h>
 
 static osMutexAttr_t msb_i2c_mutex_attr;
 
@@ -15,27 +16,26 @@ extern device_loc_t device_loc;
 osMutexId_t i2c_mutex;
 
 // reads imu reg
-static inline int imu_read_reg(uint8_t *data, uint8_t reg, uint8_t length)
+
+int32_t lsm6dso_read_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data,
+			 uint16_t len)
 {
-	return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADDRESS, reg,
-				I2C_MEMADD_SIZE_8BIT, data, length,
-				HAL_MAX_DELAY);
+	return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADD_L, reg,
+				I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY);
 }
-
-// read imu write
-static inline int imu_write_reg(uint8_t *data, uint8_t reg, uint8_t length)
+int32_t lsm6dso_write_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data,
+			  uint16_t len)
 {
-	return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADDRESS, reg,
-				 I2C_MEMADD_SIZE_8BIT, data, length,
+	return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADD_L, reg,
+				 I2C_MEMADD_SIZE_8BIT, data, len,
 				 HAL_MAX_DELAY);
 }
-
 #ifdef SENSOR_TEMP
 sht30_t temp_sensor;
 #endif
 
 #ifdef SENSOR_IMU
-lsm6dso_t imu;
+LSM6DSO_Object_t imu;
 #endif
 
 #ifdef SENSOR_TOF
@@ -58,8 +58,16 @@ int8_t msb_init()
 
 #ifdef SENSOR_IMU
 	/* Initialize the IMU */
-	assert(!lsm6dso_init(&imu, imu_read_reg,
-			     imu_write_reg)); /* This is always connected */
+	assert(!LSM6DSO_Init(&imu)); /* This is always connected */
+
+	/* Setup IMU Accelerometer */
+	LSM6DSO_ACC_Enable(&imu);
+
+	/* Setup IMU Gyroscope */
+	LSM6DSO_GYRO_Enable(&imu);
+
+	LSM6DSO_FIFO_Set_Mode(&imu, 0);
+	LSM6DSO_ACC_Disable_Inactivity_Detection(&imu);
 #endif
 
 #ifdef SENSOR_TOF
@@ -137,40 +145,6 @@ void strain2_read(uint32_t strain2)
 }
 #endif
 
-#ifdef SENSOR_IMU
-int8_t accel_read(uint16_t accel[3])
-{
-	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-	if (mut_stat)
-		return mut_stat;
-
-	HAL_StatusTypeDef hal_stat = lsm6dso_read_accel(&imu);
-	if (hal_stat)
-		return hal_stat;
-
-	memcpy(accel, imu.accel_data, 3);
-
-	osMutexRelease(i2c_mutex);
-	return 0;
-}
-
-int8_t gyro_read(uint16_t gyro[3])
-{
-	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-	if (mut_stat)
-		return mut_stat;
-
-	HAL_StatusTypeDef hal_stat = lsm6dso_read_gyro(&imu);
-	if (hal_stat)
-		return hal_stat;
-
-	memcpy(gyro, imu.gyro_data, 3);
-
-	osMutexRelease(i2c_mutex);
-	return 0;
-}
-#endif
-
 #ifdef SENSOR_TOF
 VL6180x_RangeData_t *range;
 int8_t distance_read(int32_t *range_mm)
@@ -209,4 +183,20 @@ int8_t vcc5_en_write(bool status)
 {
 	HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status);
 	return 0;
-}
\ No newline at end of file
+}
+
+#ifdef SENSOR_IMU
+int32_t imu_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx,
+		     lsm6dso_md_t *imu_md_temp, lsm6dso_data_t *imu_data_temp)
+{
+	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
+	if (mut_stat)
+		return mut_stat;
+	HAL_StatusTypeDef hal_stat =
+		lsm6dso_data_get(ctx, aux_ctx, imu_md_temp, imu_data_temp);
+	osMutexRelease(i2c_mutex);
+	if (hal_stat)
+		return hal_stat;
+	return 0;
+}
+#endif
\ No newline at end of file
diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base
index e230b02..6427e59 160000
--- a/Drivers/Embedded-Base
+++ b/Drivers/Embedded-Base
@@ -1 +1 @@
-Subproject commit e230b029be3c05706e84a8cc7abcd3f702f0c282
+Subproject commit 6427e594510a830b62185d601df8a5c3815745d7
diff --git a/Makefile b/Makefile
index cd2fe87..615e3c4 100644
--- a/Makefile
+++ b/Makefile
@@ -73,6 +73,7 @@ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c \
 Drivers/Embedded-Base/platforms/stm32f405/src/can.c \
 Drivers/Embedded-Base/general/src/lsm6dso.c \
+Drivers/Embedded-Base/general/src/lsm6dso_reg.c \
 Drivers/Embedded-Base/general/src/vl6180x_api.c \
 Drivers/Embedded-Base/general/src/vl6180x_i2c.c \
 Drivers/Embedded-Base/middleware/src/c_utils.c \