From fad789c425e3c5151643f2987ec28d905896dfea Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Sun, 17 Nov 2024 20:25:53 -0500
Subject: [PATCH 01/12] Integrated new STM32 Driver

---
 Core/Inc/msb.h     |  4 ++--
 Core/Src/monitor.c | 34 +++++++++++++++++-----------------
 Core/Src/msb.c     | 41 ++++++++++++++++++++---------------------
 Makefile           |  1 +
 4 files changed, 40 insertions(+), 40 deletions(-)

diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h
index d58773d..a3ba1af 100644
--- a/Core/Inc/msb.h
+++ b/Core/Inc/msb.h
@@ -24,9 +24,9 @@ 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 accel_read(LSM6DSO_Axes_t* accel);
 
-int8_t gyro_read(uint16_t gyro[3]);
+int8_t gyro_read(LSM6DSO_Axes_t* gyro);
 #endif
 
 #ifdef SENSOR_TOF
diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index 1098260..f5776f5 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -88,7 +88,7 @@ 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,
 };
 
@@ -105,38 +105,38 @@ 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 };
+	LSM6DSO_Axes_t accel_data_temp = { 0 };
+	LSM6DSO_Axes_t gyro_data_temp = (LSM6DSO_Axes_t) { 0 };
 
 	for (;;) {
 		/* Take measurement */
 
-		if (accel_read(accel_data_temp)) {
+		if (accel_read(&accel_data_temp)) {
 			printf("Failed to get IMU acceleration\r\n");
 		}
 
-		if (gyro_read(gyro_data_temp)) {
+		if (gyro_read(&gyro_data_temp)) {
 			printf("Failed to get IMU gyroscope\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 = accel_data_temp.x;
+		accel_data.accel_y = accel_data_temp.y;
+		accel_data.accel_z = accel_data_temp.z;
+		gyro_data.gyro_x = gyro_data_temp.x;
+		gyro_data.gyro_y = gyro_data_temp.y;
+		gyro_data.gyro_z = gyro_data_temp.z;
 
 #ifdef LOG_VERBOSE
 		printf("IMU Accel x: %d y: %d z: %d \r\n", accel_data.accel_x,
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 9c7be3f..226795f 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -1,6 +1,7 @@
 #include "msb.h"
 #include "main.h"
 #include "lsm6dso.h"
+#include "lsm6dso_reg.h"
 #include <assert.h>
 #include <stdlib.h>
 #include <string.h>
@@ -15,27 +16,25 @@ 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,
+	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,
-				 HAL_MAX_DELAY);
+	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 +57,7 @@ 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 */
 #endif
 
 #ifdef SENSOR_TOF
@@ -85,6 +83,9 @@ int8_t msb_init()
 	i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
 	assert(i2c_mutex);
 
+	LSM6DSO_ACC_Enable(&imu);
+	LSM6DSO_GYRO_Enable(&imu);
+	LSM6DSO_GYRO_Set_Power_Mode(&imu, LSM6DSO_GY_HIGH_PERFORMANCE);
 	return 0;
 }
 
@@ -138,33 +139,31 @@ void strain2_read(uint32_t strain2)
 #endif
 
 #ifdef SENSOR_IMU
-int8_t accel_read(uint16_t accel[3])
+int8_t accel_read(LSM6DSO_Axes_t* accel)
 {
 	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
 	if (mut_stat)
 		return mut_stat;
-
-	HAL_StatusTypeDef hal_stat = lsm6dso_read_accel(&imu);
+	HAL_StatusTypeDef hal_stat = LSM6DSO_ACC_GetAxes(&imu, accel);
 	if (hal_stat)
 		return hal_stat;
 
-	memcpy(accel, imu.accel_data, 3);
+	//memcpy(accel, imu.accel, 3);
 
 	osMutexRelease(i2c_mutex);
 	return 0;
 }
 
-int8_t gyro_read(uint16_t gyro[3])
+int8_t gyro_read(LSM6DSO_Axes_t* gyro)
 {
 	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
 	if (mut_stat)
 		return mut_stat;
-
-	HAL_StatusTypeDef hal_stat = lsm6dso_read_gyro(&imu);
+	HAL_StatusTypeDef hal_stat = LSM6DSO_GYRO_GetAxes(&imu, gyro);
 	if (hal_stat)
 		return hal_stat;
 
-	memcpy(gyro, imu.gyro_data, 3);
+	//memcpy(gyro, imu.gyro, 3);
 
 	osMutexRelease(i2c_mutex);
 	return 0;
diff --git a/Makefile b/Makefile
index 7985941..612ceb5 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 \

From 329d975adb00cdffcf6824e7e3080bc9dfad7b8b Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Wed, 4 Dec 2024 20:01:55 -0500
Subject: [PATCH 02/12] Updated lsm6dso driver and interfaced with IMU

---
 Core/Src/monitor.c | 42 +++++++++++++++++++++++++++---------------
 Core/Src/msb.c     |  7 ++++++-
 2 files changed, 33 insertions(+), 16 deletions(-)

diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index f5776f5..03c3345 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -116,33 +116,44 @@ void vIMUMonitor(void *pv_params)
 		int16_t gyro_z;
 	} gyro_data;
 
-	LSM6DSO_Axes_t accel_data_temp = { 0 };
-	LSM6DSO_Axes_t gyro_data_temp = (LSM6DSO_Axes_t) { 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;
 
-		if (gyro_read(&gyro_data_temp)) {
-			printf("Failed to get IMU gyroscope\r\n");
+	/* 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;
+
+	for (;;) {
+		/* Take measurement */
+		if (lsm6dso_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_temp.x;
-		accel_data.accel_y = accel_data_temp.y;
-		accel_data.accel_z = accel_data_temp.z;
-		gyro_data.gyro_x = gyro_data_temp.x;
-		gyro_data.gyro_y = gyro_data_temp.y;
-		gyro_data.gyro_z = gyro_data_temp.z;
+		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 */
@@ -152,6 +163,7 @@ void vIMUMonitor(void *pv_params)
 		endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x));
 		endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y));
 		endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z));
+		endian_swap(&temperature_data.temp, sizeof(temperature_data.temp));
 
 		/* Send CAN message */
 		memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len);
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 226795f..ab62390 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -83,9 +83,14 @@ int8_t msb_init()
 	i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
 	assert(i2c_mutex);
 
+	/* Setup IMU Accelerometer */
 	LSM6DSO_ACC_Enable(&imu);
+
+	/* Setup IMU Gyroscope */
 	LSM6DSO_GYRO_Enable(&imu);
-	LSM6DSO_GYRO_Set_Power_Mode(&imu, LSM6DSO_GY_HIGH_PERFORMANCE);
+
+	LSM6DSO_FIFO_Set_Mode(&imu, 0);
+	LSM6DSO_ACC_Disable_Inactivity_Detection(&imu);
 	return 0;
 }
 

From 226ddcb74d8aa6957d62d3d66664df78ff770efc Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Wed, 4 Dec 2024 20:16:56 -0500
Subject: [PATCH 03/12] Updated lsm6dso driver and interfaced with IMU

---
 Core/Inc/msb.h     | 4 ++--
 Core/Src/monitor.c | 1 -
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h
index a3ba1af..650c84b 100644
--- a/Core/Inc/msb.h
+++ b/Core/Inc/msb.h
@@ -24,9 +24,9 @@ int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity);
 #endif
 
 #ifdef SENSOR_IMU
-int8_t accel_read(LSM6DSO_Axes_t* accel);
+int8_t accel_read(LSM6DSO_Axes_t *accel);
 
-int8_t gyro_read(LSM6DSO_Axes_t* gyro);
+int8_t gyro_read(LSM6DSO_Axes_t *gyro);
 #endif
 
 #ifdef SENSOR_TOF
diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index 03c3345..2789660 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -163,7 +163,6 @@ void vIMUMonitor(void *pv_params)
 		endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x));
 		endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y));
 		endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z));
-		endian_swap(&temperature_data.temp, sizeof(temperature_data.temp));
 
 		/* Send CAN message */
 		memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len);

From 70a936f8b18164a772e486ab076dc0fe0fb8ec8f Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Wed, 4 Dec 2024 20:20:03 -0500
Subject: [PATCH 04/12] Updated lsm6dso driver and interfaced with IMU

---
 Core/Src/msb.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index ab62390..cc50175 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -144,7 +144,7 @@ void strain2_read(uint32_t strain2)
 #endif
 
 #ifdef SENSOR_IMU
-int8_t accel_read(LSM6DSO_Axes_t* accel)
+int8_t accel_read(LSM6DSO_Axes_t *accel)
 {
 	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
 	if (mut_stat)
@@ -159,7 +159,7 @@ int8_t accel_read(LSM6DSO_Axes_t* accel)
 	return 0;
 }
 
-int8_t gyro_read(LSM6DSO_Axes_t* gyro)
+int8_t gyro_read(LSM6DSO_Axes_t *gyro)
 {
 	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
 	if (mut_stat)

From cee1a2740776a451fc1e4b75da49d6d45eb76e65 Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Wed, 4 Dec 2024 22:11:41 -0500
Subject: [PATCH 05/12] Removed unnecessary methods

---
 Core/Inc/msb.h     |  8 ++-----
 Core/Src/monitor.c |  2 +-
 Core/Src/msb.c     | 54 ++++++++++++----------------------------------
 3 files changed, 17 insertions(+), 47 deletions(-)

diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h
index 650c84b..836919a 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(LSM6DSO_Axes_t *accel);
-
-int8_t gyro_read(LSM6DSO_Axes_t *gyro);
-#endif
-
 #ifdef SENSOR_TOF
 int8_t distance_read(int32_t *range_mm);
 #endif
@@ -39,6 +33,8 @@ 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 2789660..7b9bb76 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -135,7 +135,7 @@ void vIMUMonitor(void *pv_params)
 
 	for (;;) {
 		/* Take measurement */
-		if (lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp)) {
+		if (imu_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp)) {
 			printf("Failed to get IMU data \r\n");
 		}
 
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index cc50175..7e9972b 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -58,6 +58,15 @@ int8_t msb_init()
 #ifdef SENSOR_IMU
 	/* Initialize the IMU */
 	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
@@ -83,14 +92,6 @@ int8_t msb_init()
 	i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
 	assert(i2c_mutex);
 
-	/* 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);
 	return 0;
 }
 
@@ -143,38 +144,6 @@ void strain2_read(uint32_t strain2)
 }
 #endif
 
-#ifdef SENSOR_IMU
-int8_t accel_read(LSM6DSO_Axes_t *accel)
-{
-	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-	if (mut_stat)
-		return mut_stat;
-	HAL_StatusTypeDef hal_stat = LSM6DSO_ACC_GetAxes(&imu, accel);
-	if (hal_stat)
-		return hal_stat;
-
-	//memcpy(accel, imu.accel, 3);
-
-	osMutexRelease(i2c_mutex);
-	return 0;
-}
-
-int8_t gyro_read(LSM6DSO_Axes_t *gyro)
-{
-	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-	if (mut_stat)
-		return mut_stat;
-	HAL_StatusTypeDef hal_stat = LSM6DSO_GYRO_GetAxes(&imu, gyro);
-	if (hal_stat)
-		return hal_stat;
-
-	//memcpy(gyro, imu.gyro, 3);
-
-	osMutexRelease(i2c_mutex);
-	return 0;
-}
-#endif
-
 #ifdef SENSOR_TOF
 VL6180x_RangeData_t *range;
 int8_t distance_read(int32_t *range_mm)
@@ -213,4 +182,9 @@ int8_t vcc5_en_write(bool status)
 {
 	HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status);
 	return 0;
+}
+
+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)
+{
+	lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp);
 }
\ No newline at end of file

From 9c8efcfdc0c4b5ea29b88fc802cf874a41fcada2 Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Wed, 4 Dec 2024 22:28:13 -0500
Subject: [PATCH 06/12] Mutex for imu_data_get

---
 Core/Src/msb.c | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 7e9972b..4a4e903 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -47,15 +47,13 @@ uint32_t adc1_buf[3];
 
 int8_t msb_init()
 {
-#ifdef SENSOR_TEMP
+#ifdef SENSOR_TEMP && SENSOR_IMU
 	/* Initialize the Onboard Temperature Sensor */
 	temp_sensor = (sht30_t){
 		.i2c_handle = &hi2c3,
 	};
 	assert(!sht30_init(&temp_sensor)); /* This is always connected */
-#endif
 
-#ifdef SENSOR_IMU
 	/* Initialize the IMU */
 	assert(!LSM6DSO_Init(&imu)); /* This is always connected */
 
@@ -184,7 +182,18 @@ int8_t vcc5_en_write(bool status)
 	return 0;
 }
 
+#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)
 {
-	lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp);
-}
\ No newline at end of file
+	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);;
+	if (hal_stat)
+		return hal_stat;
+
+	//memcpy(gyro, imu.gyro, 3);
+
+	osMutexRelease(i2c_mutex);
+}
+#endif
\ No newline at end of file

From 23f86fb225ecc000916c7dec8b79c7c912942ce6 Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Wed, 4 Dec 2024 23:08:17 -0500
Subject: [PATCH 07/12] Split ifdef block

---
 Core/Src/msb.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 4a4e903..bd6d0d1 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -47,13 +47,15 @@ uint32_t adc1_buf[3];
 
 int8_t msb_init()
 {
-#ifdef SENSOR_TEMP && SENSOR_IMU
+#ifdef SENSOR_TEMP
 	/* Initialize the Onboard Temperature Sensor */
 	temp_sensor = (sht30_t){
 		.i2c_handle = &hi2c3,
 	};
 	assert(!sht30_init(&temp_sensor)); /* This is always connected */
+#endif
 
+#ifdef SENSOR_IMU
 	/* Initialize the IMU */
 	assert(!LSM6DSO_Init(&imu)); /* This is always connected */
 

From 12b914188ff9af034541122cbc22c534f9225435 Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Thu, 5 Dec 2024 20:17:30 -0500
Subject: [PATCH 08/12] fixup by jack lol

---
 Core/Src/msb.c        | 2 +-
 Drivers/Embedded-Base | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index bd6d0d1..28423c7 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -190,7 +190,7 @@ int32_t imu_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, lsm6dso_md_t *imu
 	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);;
+	HAL_StatusTypeDef hal_stat = lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp);
 	if (hal_stat)
 		return hal_stat;
 
diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base
index 20415db..6427e59 160000
--- a/Drivers/Embedded-Base
+++ b/Drivers/Embedded-Base
@@ -1 +1 @@
-Subproject commit 20415dbef0a34e5ffce356a2f68d9d8ad8108e86
+Subproject commit 6427e594510a830b62185d601df8a5c3815745d7

From 7b5bc3c6b06fb62f8eb48ab65d362192e38fe4dc Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Thu, 5 Dec 2024 20:42:24 -0500
Subject: [PATCH 09/12] Clang formatting

---
 Core/Src/monitor.c | 406 +++++++++++++++++++++------------------------
 Core/Src/msb.c     | 207 +++++++++++------------
 2 files changed, 291 insertions(+), 322 deletions(-)

diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index 7b9bb76..ed10477 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -14,294 +14,274 @@
 
 extern device_loc_t device_loc;
 
-uint16_t convert_can(uint16_t original_value, device_loc_t mode)
-{
-	switch (mode) {
-	case DEVICE_FRONT_LEFT:
-		return original_value;
-	case DEVICE_FRONT_RIGHT:
-		return original_value + 0x20;
-	case DEVICE_BACK_LEFT:
-		return original_value + 0x40;
-	case DEVICE_BACK_RIGHT:
-		return original_value + 0x60;
-	default:
-		return original_value;
-	}
+uint16_t convert_can(uint16_t original_value, device_loc_t mode) {
+  switch (mode) {
+  case DEVICE_FRONT_LEFT:
+    return original_value;
+  case DEVICE_FRONT_RIGHT:
+    return original_value + 0x20;
+  case DEVICE_BACK_LEFT:
+    return original_value + 0x40;
+  case DEVICE_BACK_RIGHT:
+    return original_value + 0x60;
+  default:
+    return original_value;
+  }
 }
 
 #ifdef SENSOR_TEMP
 osThreadId_t temp_monitor_handle;
 const osThreadAttr_t temp_monitor_attributes = {
-	.name = "TempMonitor",
-	.stack_size = 64 * 16,
-	.priority = (osPriority_t)osPriorityHigh1,
+    .name = "TempMonitor",
+    .stack_size = 64 * 16,
+    .priority = (osPriority_t)osPriorityHigh1,
 };
 
-void vTempMonitor(void *pv_params)
-{
-	can_msg_t temp_sensor_msg = { .id = convert_can(CANID_TEMP_SENSOR,
-							device_loc),
-				      .len = 4,
-				      .data = { 0 } };
+void vTempMonitor(void *pv_params) {
+  can_msg_t temp_sensor_msg = {
+      .id = convert_can(CANID_TEMP_SENSOR, device_loc), .len = 4, .data = {0}};
 
-	struct __attribute__((__packed__)) {
-		uint16_t temp;
-		uint16_t humidity;
-	} temp_sensor_data;
+  struct __attribute__((__packed__)) {
+    uint16_t temp;
+    uint16_t humidity;
+  } temp_sensor_data;
 
-	uint16_t temp_dat = 0;
-	uint16_t humidity_dat = 0;
+  uint16_t temp_dat = 0;
+  uint16_t humidity_dat = 0;
 
-	for (;;) {
-		if (central_temp_measure(&temp_dat, &humidity_dat)) {
-			printf("Failed to get temp\r\n");
-		}
+  for (;;) {
+    if (central_temp_measure(&temp_dat, &humidity_dat)) {
+      printf("Failed to get temp\r\n");
+    }
 
-		temp_sensor_data.temp = temp_dat;
-		temp_sensor_data.humidity = humidity_dat;
+    temp_sensor_data.temp = temp_dat;
+    temp_sensor_data.humidity = humidity_dat;
 
 #ifdef LOG_VERBOSE
-		printf("Board Temperature:\t%d\r\n", temp_sensor_data.temp);
-		printf("Board Humidity:\t%d\r\n", temp_sensor_data.humidity);
+    printf("Board Temperature:\t%d\r\n", temp_sensor_data.temp);
+    printf("Board Humidity:\t%d\r\n", temp_sensor_data.humidity);
 #endif
 
-		endian_swap(&temp_sensor_data.temp,
-			    sizeof(temp_sensor_data.temp));
-		endian_swap(&temp_sensor_data.humidity,
-			    sizeof(temp_sensor_data.humidity));
-
-		memcpy(temp_sensor_msg.data, &temp_sensor_data,
-		       temp_sensor_msg.len);
-		/* Send CAN message */
-		if (queue_can_msg(temp_sensor_msg)) {
-			printf("Failed to send CAN message\r\n");
-		}
-
-		/* Yield to other tasks */
-		osDelay(DELAY_TEMP_SENSOR_REFRESH);
-	}
+    endian_swap(&temp_sensor_data.temp, sizeof(temp_sensor_data.temp));
+    endian_swap(&temp_sensor_data.humidity, sizeof(temp_sensor_data.humidity));
+
+    memcpy(temp_sensor_msg.data, &temp_sensor_data, temp_sensor_msg.len);
+    /* Send CAN message */
+    if (queue_can_msg(temp_sensor_msg)) {
+      printf("Failed to send CAN message\r\n");
+    }
+
+    /* Yield to other tasks */
+    osDelay(DELAY_TEMP_SENSOR_REFRESH);
+  }
 }
 #endif
 
 #ifdef SENSOR_IMU
 osThreadId_t imu_monitor_handle;
 const osThreadAttr_t imu_monitor_attributes = {
-	.name = "IMUMonitor",
-	.stack_size = 128 * 8,
-	.priority = (osPriority_t)osPriorityHigh,
+    .name = "IMUMonitor",
+    .stack_size = 128 * 8,
+    .priority = (osPriority_t)osPriorityHigh,
 };
 
-void vIMUMonitor(void *pv_params)
-{
-	//const uint8_t num_samples = 10;
-	can_msg_t imu_accel_msg = { .id = convert_can(CANID_IMU_ACCEL,
-						      device_loc),
-				    .len = 6,
-				    .data = { 0 } };
-	can_msg_t imu_gyro_msg = { .id = convert_can(CANID_IMU_GYRO,
-						     device_loc),
-				   .len = 6,
-				   .data = { 0 } };
-
-	struct __attribute__((__packed__)) {
-		int16_t accel_x;
-		int16_t accel_y;
-		int16_t accel_z;
-	} accel_data;
-
-	struct __attribute__((__packed__)) {
-		int16_t gyro_x;
-		int16_t gyro_y;
-		int16_t gyro_z;
-	} gyro_data;
-
-	struct __attribute__((__packed__)) {
-		float_t temp;
-	} temperature_data;  
-
-	stmdev_ctx_t ctx;
-	stmdev_ctx_t aux_ctx;
-	//int16_t temperature_data_temp;
-
-	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;
-
-	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 = 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;
+void vIMUMonitor(void *pv_params) {
+  // const uint8_t num_samples = 10;
+  can_msg_t imu_accel_msg = {
+      .id = convert_can(CANID_IMU_ACCEL, device_loc), .len = 6, .data = {0}};
+  can_msg_t imu_gyro_msg = {
+      .id = convert_can(CANID_IMU_GYRO, device_loc), .len = 6, .data = {0}};
+
+  struct __attribute__((__packed__)) {
+    int16_t accel_x;
+    int16_t accel_y;
+    int16_t accel_z;
+  } accel_data;
+
+  struct __attribute__((__packed__)) {
+    int16_t gyro_x;
+    int16_t gyro_y;
+    int16_t gyro_z;
+  } gyro_data;
+
+  struct __attribute__((__packed__)) {
+    float_t temp;
+  } temperature_data;
+
+  stmdev_ctx_t ctx;
+  stmdev_ctx_t aux_ctx;
+  // int16_t temperature_data_temp;
+
+  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;
+
+  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 = 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);
+    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 */
-		endian_swap(&accel_data.accel_x, sizeof(accel_data.accel_x));
-		endian_swap(&accel_data.accel_y, sizeof(accel_data.accel_y));
-		endian_swap(&accel_data.accel_z, sizeof(accel_data.accel_z));
-		endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x));
-		endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y));
-		endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z));
-
-		/* Send CAN message */
-		memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len);
-		if (queue_can_msg(imu_accel_msg)) {
-			printf("Failed to send CAN message\r\n");
-		}
-
-		memcpy(imu_gyro_msg.data, &gyro_data, imu_gyro_msg.len);
-		if (queue_can_msg(imu_gyro_msg)) {
-			printf("Failed to send CAN message\r\n");
-		}
-
-		/* Yield to other tasks */
-		osDelay(DELAY_IMU_REFRESH);
-	}
+    /* convert to big endian */
+    endian_swap(&accel_data.accel_x, sizeof(accel_data.accel_x));
+    endian_swap(&accel_data.accel_y, sizeof(accel_data.accel_y));
+    endian_swap(&accel_data.accel_z, sizeof(accel_data.accel_z));
+    endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x));
+    endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y));
+    endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z));
+
+    /* Send CAN message */
+    memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len);
+    if (queue_can_msg(imu_accel_msg)) {
+      printf("Failed to send CAN message\r\n");
+    }
+
+    memcpy(imu_gyro_msg.data, &gyro_data, imu_gyro_msg.len);
+    if (queue_can_msg(imu_gyro_msg)) {
+      printf("Failed to send CAN message\r\n");
+    }
+
+    /* Yield to other tasks */
+    osDelay(DELAY_IMU_REFRESH);
+  }
 }
 #endif
 
 #ifdef SENSOR_TOF
 osThreadId_t tof_monitor_handle;
 const osThreadAttr_t tof_monitor_attributes = {
-	.name = "TOFMonitor",
-	.stack_size = 128 * 8,
-	.priority = (osPriority_t)osPriorityHigh,
+    .name = "TOFMonitor",
+    .stack_size = 128 * 8,
+    .priority = (osPriority_t)osPriorityHigh,
 };
 
-void vTOFMonitor(void *pv_params)
-{
-	can_msg_t range_msg = { .id = convert_can(CANID_TOF, device_loc),
-				.len = 4,
-				.data = { 0 } };
+void vTOFMonitor(void *pv_params) {
+  can_msg_t range_msg = {
+      .id = convert_can(CANID_TOF, device_loc), .len = 4, .data = {0}};
 
-	int32_t range;
+  int32_t range;
 
-	for (;;) {
-		if (distance_read(&range)) {
-			printf("failed to read distance!\r\n");
-			continue;
-		}
+  for (;;) {
+    if (distance_read(&range)) {
+      printf("failed to read distance!\r\n");
+      continue;
+    }
 
 #ifdef LOG_VERBOSE
-		printf("Range is: %ld\r\n", range);
+    printf("Range is: %ld\r\n", range);
 #endif
 
-		endian_swap(&range, sizeof(range));
+    endian_swap(&range, sizeof(range));
 
-		memcpy(range_msg.data, &range, range_msg.len);
-		/* Send CAN message */
-		if (queue_can_msg(range_msg)) {
-			printf("Failed to send CAN message\r\n");
-		}
+    memcpy(range_msg.data, &range, range_msg.len);
+    /* Send CAN message */
+    if (queue_can_msg(range_msg)) {
+      printf("Failed to send CAN message\r\n");
+    }
 
-		osDelay(DELAY_TOF_REFRESH);
-	}
+    osDelay(DELAY_TOF_REFRESH);
+  }
 }
 #endif
 
 #ifdef SENSOR_SHOCKPOT
 osThreadId_t shockpot_monitor_handle;
 const osThreadAttr_t shockpot_monitor_attributes = {
-	.name = "ShockpotMonitor",
-	.stack_size = 64 * 8,
-	.priority = (osPriority_t)osPriorityHigh1,
+    .name = "ShockpotMonitor",
+    .stack_size = 64 * 8,
+    .priority = (osPriority_t)osPriorityHigh1,
 };
 
-void vShockpotMonitor(void *pv_params)
-{
-	can_msg_t shockpot_msg = { .id = convert_can(CANID_SHOCK_SENSE,
-						     device_loc),
-				   .len = 4,
-				   .data = { 0 } };
+void vShockpotMonitor(void *pv_params) {
+  can_msg_t shockpot_msg = {
+      .id = convert_can(CANID_SHOCK_SENSE, device_loc), .len = 4, .data = {0}};
 
-	uint32_t shock_value = 0;
+  uint32_t shock_value = 0;
 
-	for (;;) {
-		shockpot_read(shock_value);
+  for (;;) {
+    shockpot_read(shock_value);
 
 #ifdef LOG_VERBOSE
-		printf("Shock value:\t%ld\r\n", shock_value);
+    printf("Shock value:\t%ld\r\n", shock_value);
 #endif
 
-		endian_swap(&shock_value, sizeof(shock_value));
+    endian_swap(&shock_value, sizeof(shock_value));
 
-		memcpy(shockpot_msg.data, &shock_value, shockpot_msg.len);
-		/* Send CAN message */
-		if (queue_can_msg(shockpot_msg)) {
-			printf("Failed to send CAN message\r\n");
-		}
+    memcpy(shockpot_msg.data, &shock_value, shockpot_msg.len);
+    /* Send CAN message */
+    if (queue_can_msg(shockpot_msg)) {
+      printf("Failed to send CAN message\r\n");
+    }
 
-		/* Yield to other tasks */
-		osDelay(DELAY_SHOCKPOT_REFRESH);
-	}
+    /* Yield to other tasks */
+    osDelay(DELAY_SHOCKPOT_REFRESH);
+  }
 }
 #endif
 
 #ifdef SENSOR_STRAIN
 osThreadId_t strain_monitor_handle;
 const osThreadAttr_t strain_monitor_attributes = {
-	.name = "StrainMonitor",
-	.stack_size = 64 * 8,
-	.priority = (osPriority_t)osPriorityHigh1,
+    .name = "StrainMonitor",
+    .stack_size = 64 * 8,
+    .priority = (osPriority_t)osPriorityHigh1,
 };
 
-void vStrainMonitor(void *pv_params)
-{
-	can_msg_t strain_msg = { .id = convert_can(CANID_STRAIN_SENSE,
-						   device_loc),
-				 .len = 8,
-				 .data = { 0 } };
+void vStrainMonitor(void *pv_params) {
+  can_msg_t strain_msg = {
+      .id = convert_can(CANID_STRAIN_SENSE, device_loc), .len = 8, .data = {0}};
 
-	struct __attribute__((__packed__)) {
-		uint32_t strain1;
-		uint32_t strain2;
-	} strain_data;
+  struct __attribute__((__packed__)) {
+    uint32_t strain1;
+    uint32_t strain2;
+  } strain_data;
 
-	uint32_t strain1_dat = 0;
-	uint32_t strain2_dat = 0;
-	for (;;) {
-		strain1_read(strain1_dat);
-		strain2_read(strain2_dat);
+  uint32_t strain1_dat = 0;
+  uint32_t strain2_dat = 0;
+  for (;;) {
+    strain1_read(strain1_dat);
+    strain2_read(strain2_dat);
 
 #ifdef LOG_VERBOSE
-		printf("Strain 1: %ld  2: %ld \r\n", strain1_dat, strain2_dat);
+    printf("Strain 1: %ld  2: %ld \r\n", strain1_dat, strain2_dat);
 #endif
 
-		strain_data.strain1 = strain1_dat;
-		strain_data.strain2 = strain2_dat;
+    strain_data.strain1 = strain1_dat;
+    strain_data.strain2 = strain2_dat;
 
-		endian_swap(&strain_data.strain1, sizeof(strain_data.strain1));
-		endian_swap(&strain_data.strain2, sizeof(strain_data.strain2));
+    endian_swap(&strain_data.strain1, sizeof(strain_data.strain1));
+    endian_swap(&strain_data.strain2, sizeof(strain_data.strain2));
 
-		memcpy(strain_msg.data, &strain_data, strain_msg.len);
-		/* Send CAN message */
-		if (queue_can_msg(strain_msg)) {
-			printf("Failed to send CAN message");
-		}
+    memcpy(strain_msg.data, &strain_data, strain_msg.len);
+    /* Send CAN message */
+    if (queue_can_msg(strain_msg)) {
+      printf("Failed to send CAN message");
+    }
 
-		/* Yield to other tasks */
-		osDelay(DELAY_SHOCKPOT_REFRESH);
-	}
+    /* Yield to other tasks */
+    osDelay(DELAY_SHOCKPOT_REFRESH);
+  }
 }
 #endif
\ No newline at end of file
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 28423c7..545cc49 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -1,11 +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;
 
@@ -17,17 +17,15 @@ osMutexId_t i2c_mutex;
 
 // reads imu reg
 
-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_ADD_L, reg,
-				I2C_MEMADD_SIZE_8BIT, data, len,
-				HAL_MAX_DELAY);
+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_ADD_L, reg, I2C_MEMADD_SIZE_8BIT,
+                          data, len, HAL_MAX_DELAY);
 }
-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_ADD_L, reg,
-	I2C_MEMADD_SIZE_8BIT, data, len,
-	HAL_MAX_DELAY);
+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_ADD_L, reg, I2C_MEMADD_SIZE_8BIT,
+                           data, len, HAL_MAX_DELAY);
 }
 #ifdef SENSOR_TEMP
 sht30_t temp_sensor;
@@ -45,157 +43,148 @@ VL6180xDev_t tof;
 uint32_t adc1_buf[3];
 #endif
 
-int8_t msb_init()
-{
+int8_t msb_init() {
 #ifdef SENSOR_TEMP
-	/* Initialize the Onboard Temperature Sensor */
-	temp_sensor = (sht30_t){
-		.i2c_handle = &hi2c3,
-	};
-	assert(!sht30_init(&temp_sensor)); /* This is always connected */
+  /* Initialize the Onboard Temperature Sensor */
+  temp_sensor = (sht30_t){
+      .i2c_handle = &hi2c3,
+  };
+  assert(!sht30_init(&temp_sensor)); /* This is always connected */
 #endif
 
 #ifdef SENSOR_IMU
-	/* Initialize the IMU */
-	assert(!LSM6DSO_Init(&imu)); /* This is always connected */
+  /* Initialize the IMU */
+  assert(!LSM6DSO_Init(&imu)); /* This is always connected */
 
-	/* Setup IMU Accelerometer */
-	LSM6DSO_ACC_Enable(&imu);
+  /* Setup IMU Accelerometer */
+  LSM6DSO_ACC_Enable(&imu);
 
-	/* Setup IMU Gyroscope */
-	LSM6DSO_GYRO_Enable(&imu);
+  /* Setup IMU Gyroscope */
+  LSM6DSO_GYRO_Enable(&imu);
 
-	LSM6DSO_FIFO_Set_Mode(&imu, 0);
-	LSM6DSO_ACC_Disable_Inactivity_Detection(&imu);
+  LSM6DSO_FIFO_Set_Mode(&imu, 0);
+  LSM6DSO_ACC_Disable_Inactivity_Detection(&imu);
 #endif
 
 #ifdef SENSOR_TOF
-	/* Initialize the ToF sensor */
-	struct MyDev_t tof_get = {
-		.i2c_bus_num = 0x29 << 1,
-		.i2c_handle = &hi2c3,
-	};
-	tof = &tof_get;
-	assert(tof);
-	osDelay(1);
-	assert(!VL6180x_WaitDeviceBooted(tof));
-	assert(!VL6180x_InitData(tof));
-	assert(!VL6180x_Prepare(tof));
+  /* Initialize the ToF sensor */
+  struct MyDev_t tof_get = {
+      .i2c_bus_num = 0x29 << 1,
+      .i2c_handle = &hi2c3,
+  };
+  tof = &tof_get;
+  assert(tof);
+  osDelay(1);
+  assert(!VL6180x_WaitDeviceBooted(tof));
+  assert(!VL6180x_InitData(tof));
+  assert(!VL6180x_Prepare(tof));
 #endif
 
 #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN
-	assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf,
-				  sizeof(adc1_buf) / sizeof(uint32_t)));
+  assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf,
+                            sizeof(adc1_buf) / sizeof(uint32_t)));
 #endif
 
-	/* Create Mutexes */
-	i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
-	assert(i2c_mutex);
+  /* Create Mutexes */
+  i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
+  assert(i2c_mutex);
 
-	return 0;
+  return 0;
 }
 
 #ifdef SENSOR_TEMP
 /// @brief Measure the temperature and humidity of central MSB SHT30
 /// @param out
 /// @return error code
-int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity)
-{
-	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-	if (mut_stat)
-		return mut_stat;
+int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity) {
+  osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
+  if (mut_stat)
+    return mut_stat;
 
-	HAL_StatusTypeDef hal_stat = sht30_get_temp_humid(&temp_sensor);
-	if (hal_stat)
-		return hal_stat;
+  HAL_StatusTypeDef hal_stat = sht30_get_temp_humid(&temp_sensor);
+  if (hal_stat)
+    return hal_stat;
 
-	*temp = temp_sensor.temp;
-	*humidity = temp_sensor.humidity;
+  *temp = temp_sensor.temp;
+  *humidity = temp_sensor.humidity;
 
-	osMutexRelease(i2c_mutex);
+  osMutexRelease(i2c_mutex);
 
-	return 0;
+  return 0;
 }
 #endif
 
 #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN
-void adc1_read(uint32_t result_buf[3])
-{
-	memcpy(result_buf, adc1_buf, sizeof(adc1_buf));
+void adc1_read(uint32_t result_buf[3]) {
+  memcpy(result_buf, adc1_buf, sizeof(adc1_buf));
 }
 #endif
 
 #ifdef SENSOR_SHOCKPOT
-void shockpot_read(uint32_t shockpot_sense)
-{
-	memcpy((uint32_t *)shockpot_sense, adc1_buf, sizeof(shockpot_sense));
+void shockpot_read(uint32_t shockpot_sense) {
+  memcpy((uint32_t *)shockpot_sense, adc1_buf, sizeof(shockpot_sense));
 }
 #endif
 
 #ifdef SENSOR_STRAIN
-void strain1_read(uint32_t strain1)
-{
-	memcpy((uint32_t *)strain1, adc1_buf + 1, sizeof(strain1));
+void strain1_read(uint32_t strain1) {
+  memcpy((uint32_t *)strain1, adc1_buf + 1, sizeof(strain1));
 }
 
-void strain2_read(uint32_t strain2)
-{
-	memcpy((uint32_t *)strain2, adc1_buf + 2, sizeof(strain2));
+void strain2_read(uint32_t strain2) {
+  memcpy((uint32_t *)strain2, adc1_buf + 2, sizeof(strain2));
 }
 #endif
 
 #ifdef SENSOR_TOF
 VL6180x_RangeData_t *range;
-int8_t distance_read(int32_t *range_mm)
-{
-	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-	if (mut_stat)
-		return mut_stat;
-
-	VL6180x_RangePollMeasurement(tof, range);
-	if (range->errorStatus) {
-		printf("Error in range %s\r\n",
-		       VL6180x_RangeGetStatusErrString(range->errorStatus));
-		return range->errorStatus;
-	}
-
-	memcpy(range_mm, &range->range_mm, sizeof(range->range_mm));
-
-	osMutexRelease(i2c_mutex);
-	return 0;
+int8_t distance_read(int32_t *range_mm) {
+  osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
+  if (mut_stat)
+    return mut_stat;
+
+  VL6180x_RangePollMeasurement(tof, range);
+  if (range->errorStatus) {
+    printf("Error in range %s\r\n",
+           VL6180x_RangeGetStatusErrString(range->errorStatus));
+    return range->errorStatus;
+  }
+
+  memcpy(range_mm, &range->range_mm, sizeof(range->range_mm));
+
+  osMutexRelease(i2c_mutex);
+  return 0;
 }
 #endif
 
-int8_t debug1_write(bool status)
-{
-	HAL_GPIO_WritePin(Debug_LED_1_GPIO_Port, Debug_LED_1_Pin, status);
-	return 0;
+int8_t debug1_write(bool status) {
+  HAL_GPIO_WritePin(Debug_LED_1_GPIO_Port, Debug_LED_1_Pin, status);
+  return 0;
 }
 
-int8_t debug2_write(bool status)
-{
-	HAL_GPIO_WritePin(Debug_LED_2_GPIO_Port, Debug_LED_2_Pin, status);
-	return 0;
+int8_t debug2_write(bool status) {
+  HAL_GPIO_WritePin(Debug_LED_2_GPIO_Port, Debug_LED_2_Pin, status);
+  return 0;
 }
 
-int8_t vcc5_en_write(bool status)
-{
-	HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status);
-	return 0;
+int8_t vcc5_en_write(bool status) {
+  HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status);
+  return 0;
 }
 
 #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);
-	if (hal_stat)
-		return hal_stat;
-
-	//memcpy(gyro, imu.gyro, 3);
-
-	osMutexRelease(i2c_mutex);
+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);
+  if (hal_stat)
+    return hal_stat;
+
+  // memcpy(gyro, imu.gyro, 3);
+
+  osMutexRelease(i2c_mutex);
 }
 #endif
\ No newline at end of file

From 6cbb602e3683355522e6969a58c7495002e6920b Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Thu, 5 Dec 2024 20:44:02 -0500
Subject: [PATCH 10/12] Clang format

---
 Core/Src/monitor.c | 407 ++++++++++++++++++++++++---------------------
 Core/Src/msb.c     | 200 +++++++++++-----------
 2 files changed, 321 insertions(+), 286 deletions(-)

diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index ed10477..c9e7356 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -14,274 +14,295 @@
 
 extern device_loc_t device_loc;
 
-uint16_t convert_can(uint16_t original_value, device_loc_t mode) {
-  switch (mode) {
-  case DEVICE_FRONT_LEFT:
-    return original_value;
-  case DEVICE_FRONT_RIGHT:
-    return original_value + 0x20;
-  case DEVICE_BACK_LEFT:
-    return original_value + 0x40;
-  case DEVICE_BACK_RIGHT:
-    return original_value + 0x60;
-  default:
-    return original_value;
-  }
+uint16_t convert_can(uint16_t original_value, device_loc_t mode)
+{
+	switch (mode) {
+	case DEVICE_FRONT_LEFT:
+		return original_value;
+	case DEVICE_FRONT_RIGHT:
+		return original_value + 0x20;
+	case DEVICE_BACK_LEFT:
+		return original_value + 0x40;
+	case DEVICE_BACK_RIGHT:
+		return original_value + 0x60;
+	default:
+		return original_value;
+	}
 }
 
 #ifdef SENSOR_TEMP
 osThreadId_t temp_monitor_handle;
 const osThreadAttr_t temp_monitor_attributes = {
-    .name = "TempMonitor",
-    .stack_size = 64 * 16,
-    .priority = (osPriority_t)osPriorityHigh1,
+	.name = "TempMonitor",
+	.stack_size = 64 * 16,
+	.priority = (osPriority_t)osPriorityHigh1,
 };
 
-void vTempMonitor(void *pv_params) {
-  can_msg_t temp_sensor_msg = {
-      .id = convert_can(CANID_TEMP_SENSOR, device_loc), .len = 4, .data = {0}};
+void vTempMonitor(void *pv_params)
+{
+	can_msg_t temp_sensor_msg = { .id = convert_can(CANID_TEMP_SENSOR,
+							device_loc),
+				      .len = 4,
+				      .data = { 0 } };
 
-  struct __attribute__((__packed__)) {
-    uint16_t temp;
-    uint16_t humidity;
-  } temp_sensor_data;
+	struct __attribute__((__packed__)) {
+		uint16_t temp;
+		uint16_t humidity;
+	} temp_sensor_data;
 
-  uint16_t temp_dat = 0;
-  uint16_t humidity_dat = 0;
+	uint16_t temp_dat = 0;
+	uint16_t humidity_dat = 0;
 
-  for (;;) {
-    if (central_temp_measure(&temp_dat, &humidity_dat)) {
-      printf("Failed to get temp\r\n");
-    }
+	for (;;) {
+		if (central_temp_measure(&temp_dat, &humidity_dat)) {
+			printf("Failed to get temp\r\n");
+		}
 
-    temp_sensor_data.temp = temp_dat;
-    temp_sensor_data.humidity = humidity_dat;
+		temp_sensor_data.temp = temp_dat;
+		temp_sensor_data.humidity = humidity_dat;
 
 #ifdef LOG_VERBOSE
-    printf("Board Temperature:\t%d\r\n", temp_sensor_data.temp);
-    printf("Board Humidity:\t%d\r\n", temp_sensor_data.humidity);
+		printf("Board Temperature:\t%d\r\n", temp_sensor_data.temp);
+		printf("Board Humidity:\t%d\r\n", temp_sensor_data.humidity);
 #endif
 
-    endian_swap(&temp_sensor_data.temp, sizeof(temp_sensor_data.temp));
-    endian_swap(&temp_sensor_data.humidity, sizeof(temp_sensor_data.humidity));
-
-    memcpy(temp_sensor_msg.data, &temp_sensor_data, temp_sensor_msg.len);
-    /* Send CAN message */
-    if (queue_can_msg(temp_sensor_msg)) {
-      printf("Failed to send CAN message\r\n");
-    }
-
-    /* Yield to other tasks */
-    osDelay(DELAY_TEMP_SENSOR_REFRESH);
-  }
+		endian_swap(&temp_sensor_data.temp,
+			    sizeof(temp_sensor_data.temp));
+		endian_swap(&temp_sensor_data.humidity,
+			    sizeof(temp_sensor_data.humidity));
+
+		memcpy(temp_sensor_msg.data, &temp_sensor_data,
+		       temp_sensor_msg.len);
+		/* Send CAN message */
+		if (queue_can_msg(temp_sensor_msg)) {
+			printf("Failed to send CAN message\r\n");
+		}
+
+		/* Yield to other tasks */
+		osDelay(DELAY_TEMP_SENSOR_REFRESH);
+	}
 }
 #endif
 
 #ifdef SENSOR_IMU
 osThreadId_t imu_monitor_handle;
 const osThreadAttr_t imu_monitor_attributes = {
-    .name = "IMUMonitor",
-    .stack_size = 128 * 8,
-    .priority = (osPriority_t)osPriorityHigh,
+	.name = "IMUMonitor",
+	.stack_size = 128 * 8,
+	.priority = (osPriority_t)osPriorityHigh,
 };
 
-void vIMUMonitor(void *pv_params) {
-  // const uint8_t num_samples = 10;
-  can_msg_t imu_accel_msg = {
-      .id = convert_can(CANID_IMU_ACCEL, device_loc), .len = 6, .data = {0}};
-  can_msg_t imu_gyro_msg = {
-      .id = convert_can(CANID_IMU_GYRO, device_loc), .len = 6, .data = {0}};
-
-  struct __attribute__((__packed__)) {
-    int16_t accel_x;
-    int16_t accel_y;
-    int16_t accel_z;
-  } accel_data;
-
-  struct __attribute__((__packed__)) {
-    int16_t gyro_x;
-    int16_t gyro_y;
-    int16_t gyro_z;
-  } gyro_data;
-
-  struct __attribute__((__packed__)) {
-    float_t temp;
-  } temperature_data;
-
-  stmdev_ctx_t ctx;
-  stmdev_ctx_t aux_ctx;
-  // int16_t temperature_data_temp;
-
-  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;
-
-  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 = 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;
+void vIMUMonitor(void *pv_params)
+{
+	// const uint8_t num_samples = 10;
+	can_msg_t imu_accel_msg = { .id = convert_can(CANID_IMU_ACCEL,
+						      device_loc),
+				    .len = 6,
+				    .data = { 0 } };
+	can_msg_t imu_gyro_msg = { .id = convert_can(CANID_IMU_GYRO,
+						     device_loc),
+				   .len = 6,
+				   .data = { 0 } };
+
+	struct __attribute__((__packed__)) {
+		int16_t accel_x;
+		int16_t accel_y;
+		int16_t accel_z;
+	} accel_data;
+
+	struct __attribute__((__packed__)) {
+		int16_t gyro_x;
+		int16_t gyro_y;
+		int16_t gyro_z;
+	} gyro_data;
+
+	struct __attribute__((__packed__)) {
+		float_t temp;
+	} temperature_data;
+
+	stmdev_ctx_t ctx;
+	stmdev_ctx_t aux_ctx;
+	// int16_t temperature_data_temp;
+
+	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;
+
+	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 = 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);
+		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 */
-    endian_swap(&accel_data.accel_x, sizeof(accel_data.accel_x));
-    endian_swap(&accel_data.accel_y, sizeof(accel_data.accel_y));
-    endian_swap(&accel_data.accel_z, sizeof(accel_data.accel_z));
-    endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x));
-    endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y));
-    endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z));
-
-    /* Send CAN message */
-    memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len);
-    if (queue_can_msg(imu_accel_msg)) {
-      printf("Failed to send CAN message\r\n");
-    }
-
-    memcpy(imu_gyro_msg.data, &gyro_data, imu_gyro_msg.len);
-    if (queue_can_msg(imu_gyro_msg)) {
-      printf("Failed to send CAN message\r\n");
-    }
-
-    /* Yield to other tasks */
-    osDelay(DELAY_IMU_REFRESH);
-  }
+		/* convert to big endian */
+		endian_swap(&accel_data.accel_x, sizeof(accel_data.accel_x));
+		endian_swap(&accel_data.accel_y, sizeof(accel_data.accel_y));
+		endian_swap(&accel_data.accel_z, sizeof(accel_data.accel_z));
+		endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x));
+		endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y));
+		endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z));
+
+		/* Send CAN message */
+		memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len);
+		if (queue_can_msg(imu_accel_msg)) {
+			printf("Failed to send CAN message\r\n");
+		}
+
+		memcpy(imu_gyro_msg.data, &gyro_data, imu_gyro_msg.len);
+		if (queue_can_msg(imu_gyro_msg)) {
+			printf("Failed to send CAN message\r\n");
+		}
+
+		/* Yield to other tasks */
+		osDelay(DELAY_IMU_REFRESH);
+	}
 }
 #endif
 
 #ifdef SENSOR_TOF
 osThreadId_t tof_monitor_handle;
 const osThreadAttr_t tof_monitor_attributes = {
-    .name = "TOFMonitor",
-    .stack_size = 128 * 8,
-    .priority = (osPriority_t)osPriorityHigh,
+	.name = "TOFMonitor",
+	.stack_size = 128 * 8,
+	.priority = (osPriority_t)osPriorityHigh,
 };
 
-void vTOFMonitor(void *pv_params) {
-  can_msg_t range_msg = {
-      .id = convert_can(CANID_TOF, device_loc), .len = 4, .data = {0}};
+void vTOFMonitor(void *pv_params)
+{
+	can_msg_t range_msg = { .id = convert_can(CANID_TOF, device_loc),
+				.len = 4,
+				.data = { 0 } };
 
-  int32_t range;
+	int32_t range;
 
-  for (;;) {
-    if (distance_read(&range)) {
-      printf("failed to read distance!\r\n");
-      continue;
-    }
+	for (;;) {
+		if (distance_read(&range)) {
+			printf("failed to read distance!\r\n");
+			continue;
+		}
 
 #ifdef LOG_VERBOSE
-    printf("Range is: %ld\r\n", range);
+		printf("Range is: %ld\r\n", range);
 #endif
 
-    endian_swap(&range, sizeof(range));
+		endian_swap(&range, sizeof(range));
 
-    memcpy(range_msg.data, &range, range_msg.len);
-    /* Send CAN message */
-    if (queue_can_msg(range_msg)) {
-      printf("Failed to send CAN message\r\n");
-    }
+		memcpy(range_msg.data, &range, range_msg.len);
+		/* Send CAN message */
+		if (queue_can_msg(range_msg)) {
+			printf("Failed to send CAN message\r\n");
+		}
 
-    osDelay(DELAY_TOF_REFRESH);
-  }
+		osDelay(DELAY_TOF_REFRESH);
+	}
 }
 #endif
 
 #ifdef SENSOR_SHOCKPOT
 osThreadId_t shockpot_monitor_handle;
 const osThreadAttr_t shockpot_monitor_attributes = {
-    .name = "ShockpotMonitor",
-    .stack_size = 64 * 8,
-    .priority = (osPriority_t)osPriorityHigh1,
+	.name = "ShockpotMonitor",
+	.stack_size = 64 * 8,
+	.priority = (osPriority_t)osPriorityHigh1,
 };
 
-void vShockpotMonitor(void *pv_params) {
-  can_msg_t shockpot_msg = {
-      .id = convert_can(CANID_SHOCK_SENSE, device_loc), .len = 4, .data = {0}};
+void vShockpotMonitor(void *pv_params)
+{
+	can_msg_t shockpot_msg = { .id = convert_can(CANID_SHOCK_SENSE,
+						     device_loc),
+				   .len = 4,
+				   .data = { 0 } };
 
-  uint32_t shock_value = 0;
+	uint32_t shock_value = 0;
 
-  for (;;) {
-    shockpot_read(shock_value);
+	for (;;) {
+		shockpot_read(shock_value);
 
 #ifdef LOG_VERBOSE
-    printf("Shock value:\t%ld\r\n", shock_value);
+		printf("Shock value:\t%ld\r\n", shock_value);
 #endif
 
-    endian_swap(&shock_value, sizeof(shock_value));
+		endian_swap(&shock_value, sizeof(shock_value));
 
-    memcpy(shockpot_msg.data, &shock_value, shockpot_msg.len);
-    /* Send CAN message */
-    if (queue_can_msg(shockpot_msg)) {
-      printf("Failed to send CAN message\r\n");
-    }
+		memcpy(shockpot_msg.data, &shock_value, shockpot_msg.len);
+		/* Send CAN message */
+		if (queue_can_msg(shockpot_msg)) {
+			printf("Failed to send CAN message\r\n");
+		}
 
-    /* Yield to other tasks */
-    osDelay(DELAY_SHOCKPOT_REFRESH);
-  }
+		/* Yield to other tasks */
+		osDelay(DELAY_SHOCKPOT_REFRESH);
+	}
 }
 #endif
 
 #ifdef SENSOR_STRAIN
 osThreadId_t strain_monitor_handle;
 const osThreadAttr_t strain_monitor_attributes = {
-    .name = "StrainMonitor",
-    .stack_size = 64 * 8,
-    .priority = (osPriority_t)osPriorityHigh1,
+	.name = "StrainMonitor",
+	.stack_size = 64 * 8,
+	.priority = (osPriority_t)osPriorityHigh1,
 };
 
-void vStrainMonitor(void *pv_params) {
-  can_msg_t strain_msg = {
-      .id = convert_can(CANID_STRAIN_SENSE, device_loc), .len = 8, .data = {0}};
+void vStrainMonitor(void *pv_params)
+{
+	can_msg_t strain_msg = { .id = convert_can(CANID_STRAIN_SENSE,
+						   device_loc),
+				 .len = 8,
+				 .data = { 0 } };
 
-  struct __attribute__((__packed__)) {
-    uint32_t strain1;
-    uint32_t strain2;
-  } strain_data;
+	struct __attribute__((__packed__)) {
+		uint32_t strain1;
+		uint32_t strain2;
+	} strain_data;
 
-  uint32_t strain1_dat = 0;
-  uint32_t strain2_dat = 0;
-  for (;;) {
-    strain1_read(strain1_dat);
-    strain2_read(strain2_dat);
+	uint32_t strain1_dat = 0;
+	uint32_t strain2_dat = 0;
+	for (;;) {
+		strain1_read(strain1_dat);
+		strain2_read(strain2_dat);
 
 #ifdef LOG_VERBOSE
-    printf("Strain 1: %ld  2: %ld \r\n", strain1_dat, strain2_dat);
+		printf("Strain 1: %ld  2: %ld \r\n", strain1_dat, strain2_dat);
 #endif
 
-    strain_data.strain1 = strain1_dat;
-    strain_data.strain2 = strain2_dat;
+		strain_data.strain1 = strain1_dat;
+		strain_data.strain2 = strain2_dat;
 
-    endian_swap(&strain_data.strain1, sizeof(strain_data.strain1));
-    endian_swap(&strain_data.strain2, sizeof(strain_data.strain2));
+		endian_swap(&strain_data.strain1, sizeof(strain_data.strain1));
+		endian_swap(&strain_data.strain2, sizeof(strain_data.strain2));
 
-    memcpy(strain_msg.data, &strain_data, strain_msg.len);
-    /* Send CAN message */
-    if (queue_can_msg(strain_msg)) {
-      printf("Failed to send CAN message");
-    }
+		memcpy(strain_msg.data, &strain_data, strain_msg.len);
+		/* Send CAN message */
+		if (queue_can_msg(strain_msg)) {
+			printf("Failed to send CAN message");
+		}
 
-    /* Yield to other tasks */
-    osDelay(DELAY_SHOCKPOT_REFRESH);
-  }
+		/* Yield to other tasks */
+		osDelay(DELAY_SHOCKPOT_REFRESH);
+	}
 }
 #endif
\ No newline at end of file
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 545cc49..ffcdf9a 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -18,14 +18,17 @@ osMutexId_t i2c_mutex;
 // reads imu reg
 
 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_ADD_L, reg, I2C_MEMADD_SIZE_8BIT,
-                          data, len, HAL_MAX_DELAY);
+			 uint16_t len)
+{
+	return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADD_L, reg,
+				I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY);
 }
 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_ADD_L, reg, I2C_MEMADD_SIZE_8BIT,
-                           data, len, HAL_MAX_DELAY);
+			  uint16_t len)
+{
+	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;
@@ -43,148 +46,159 @@ VL6180xDev_t tof;
 uint32_t adc1_buf[3];
 #endif
 
-int8_t msb_init() {
+int8_t msb_init()
+{
 #ifdef SENSOR_TEMP
-  /* Initialize the Onboard Temperature Sensor */
-  temp_sensor = (sht30_t){
-      .i2c_handle = &hi2c3,
-  };
-  assert(!sht30_init(&temp_sensor)); /* This is always connected */
+	/* Initialize the Onboard Temperature Sensor */
+	temp_sensor = (sht30_t){
+		.i2c_handle = &hi2c3,
+	};
+	assert(!sht30_init(&temp_sensor)); /* This is always connected */
 #endif
 
 #ifdef SENSOR_IMU
-  /* Initialize the IMU */
-  assert(!LSM6DSO_Init(&imu)); /* This is always connected */
+	/* Initialize the IMU */
+	assert(!LSM6DSO_Init(&imu)); /* This is always connected */
 
-  /* Setup IMU Accelerometer */
-  LSM6DSO_ACC_Enable(&imu);
+	/* Setup IMU Accelerometer */
+	LSM6DSO_ACC_Enable(&imu);
 
-  /* Setup IMU Gyroscope */
-  LSM6DSO_GYRO_Enable(&imu);
+	/* Setup IMU Gyroscope */
+	LSM6DSO_GYRO_Enable(&imu);
 
-  LSM6DSO_FIFO_Set_Mode(&imu, 0);
-  LSM6DSO_ACC_Disable_Inactivity_Detection(&imu);
+	LSM6DSO_FIFO_Set_Mode(&imu, 0);
+	LSM6DSO_ACC_Disable_Inactivity_Detection(&imu);
 #endif
 
 #ifdef SENSOR_TOF
-  /* Initialize the ToF sensor */
-  struct MyDev_t tof_get = {
-      .i2c_bus_num = 0x29 << 1,
-      .i2c_handle = &hi2c3,
-  };
-  tof = &tof_get;
-  assert(tof);
-  osDelay(1);
-  assert(!VL6180x_WaitDeviceBooted(tof));
-  assert(!VL6180x_InitData(tof));
-  assert(!VL6180x_Prepare(tof));
+	/* Initialize the ToF sensor */
+	struct MyDev_t tof_get = {
+		.i2c_bus_num = 0x29 << 1,
+		.i2c_handle = &hi2c3,
+	};
+	tof = &tof_get;
+	assert(tof);
+	osDelay(1);
+	assert(!VL6180x_WaitDeviceBooted(tof));
+	assert(!VL6180x_InitData(tof));
+	assert(!VL6180x_Prepare(tof));
 #endif
 
 #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN
-  assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf,
-                            sizeof(adc1_buf) / sizeof(uint32_t)));
+	assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf,
+				  sizeof(adc1_buf) / sizeof(uint32_t)));
 #endif
 
-  /* Create Mutexes */
-  i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
-  assert(i2c_mutex);
+	/* Create Mutexes */
+	i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
+	assert(i2c_mutex);
 
-  return 0;
+	return 0;
 }
 
 #ifdef SENSOR_TEMP
 /// @brief Measure the temperature and humidity of central MSB SHT30
 /// @param out
 /// @return error code
-int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity) {
-  osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-  if (mut_stat)
-    return mut_stat;
+int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity)
+{
+	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
+	if (mut_stat)
+		return mut_stat;
 
-  HAL_StatusTypeDef hal_stat = sht30_get_temp_humid(&temp_sensor);
-  if (hal_stat)
-    return hal_stat;
+	HAL_StatusTypeDef hal_stat = sht30_get_temp_humid(&temp_sensor);
+	if (hal_stat)
+		return hal_stat;
 
-  *temp = temp_sensor.temp;
-  *humidity = temp_sensor.humidity;
+	*temp = temp_sensor.temp;
+	*humidity = temp_sensor.humidity;
 
-  osMutexRelease(i2c_mutex);
+	osMutexRelease(i2c_mutex);
 
-  return 0;
+	return 0;
 }
 #endif
 
 #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN
-void adc1_read(uint32_t result_buf[3]) {
-  memcpy(result_buf, adc1_buf, sizeof(adc1_buf));
+void adc1_read(uint32_t result_buf[3])
+{
+	memcpy(result_buf, adc1_buf, sizeof(adc1_buf));
 }
 #endif
 
 #ifdef SENSOR_SHOCKPOT
-void shockpot_read(uint32_t shockpot_sense) {
-  memcpy((uint32_t *)shockpot_sense, adc1_buf, sizeof(shockpot_sense));
+void shockpot_read(uint32_t shockpot_sense)
+{
+	memcpy((uint32_t *)shockpot_sense, adc1_buf, sizeof(shockpot_sense));
 }
 #endif
 
 #ifdef SENSOR_STRAIN
-void strain1_read(uint32_t strain1) {
-  memcpy((uint32_t *)strain1, adc1_buf + 1, sizeof(strain1));
+void strain1_read(uint32_t strain1)
+{
+	memcpy((uint32_t *)strain1, adc1_buf + 1, sizeof(strain1));
 }
 
-void strain2_read(uint32_t strain2) {
-  memcpy((uint32_t *)strain2, adc1_buf + 2, sizeof(strain2));
+void strain2_read(uint32_t strain2)
+{
+	memcpy((uint32_t *)strain2, adc1_buf + 2, sizeof(strain2));
 }
 #endif
 
 #ifdef SENSOR_TOF
 VL6180x_RangeData_t *range;
-int8_t distance_read(int32_t *range_mm) {
-  osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
-  if (mut_stat)
-    return mut_stat;
-
-  VL6180x_RangePollMeasurement(tof, range);
-  if (range->errorStatus) {
-    printf("Error in range %s\r\n",
-           VL6180x_RangeGetStatusErrString(range->errorStatus));
-    return range->errorStatus;
-  }
-
-  memcpy(range_mm, &range->range_mm, sizeof(range->range_mm));
-
-  osMutexRelease(i2c_mutex);
-  return 0;
+int8_t distance_read(int32_t *range_mm)
+{
+	osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever);
+	if (mut_stat)
+		return mut_stat;
+
+	VL6180x_RangePollMeasurement(tof, range);
+	if (range->errorStatus) {
+		printf("Error in range %s\r\n",
+		       VL6180x_RangeGetStatusErrString(range->errorStatus));
+		return range->errorStatus;
+	}
+
+	memcpy(range_mm, &range->range_mm, sizeof(range->range_mm));
+
+	osMutexRelease(i2c_mutex);
+	return 0;
 }
 #endif
 
-int8_t debug1_write(bool status) {
-  HAL_GPIO_WritePin(Debug_LED_1_GPIO_Port, Debug_LED_1_Pin, status);
-  return 0;
+int8_t debug1_write(bool status)
+{
+	HAL_GPIO_WritePin(Debug_LED_1_GPIO_Port, Debug_LED_1_Pin, status);
+	return 0;
 }
 
-int8_t debug2_write(bool status) {
-  HAL_GPIO_WritePin(Debug_LED_2_GPIO_Port, Debug_LED_2_Pin, status);
-  return 0;
+int8_t debug2_write(bool status)
+{
+	HAL_GPIO_WritePin(Debug_LED_2_GPIO_Port, Debug_LED_2_Pin, status);
+	return 0;
 }
 
-int8_t vcc5_en_write(bool status) {
-  HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status);
-  return 0;
+int8_t vcc5_en_write(bool status)
+{
+	HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status);
+	return 0;
 }
 
 #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);
-  if (hal_stat)
-    return hal_stat;
-
-  // memcpy(gyro, imu.gyro, 3);
-
-  osMutexRelease(i2c_mutex);
+		     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);
+	if (hal_stat)
+		return hal_stat;
+
+	// memcpy(gyro, imu.gyro, 3);
+
+	osMutexRelease(i2c_mutex);
 }
 #endif
\ No newline at end of file

From e56c71555a9ff11de6899df09208033953558b49 Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Thu, 5 Dec 2024 20:45:00 -0500
Subject: [PATCH 11/12] Clang format

---
 Core/Inc/msb.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h
index 836919a..9435f15 100644
--- a/Core/Inc/msb.h
+++ b/Core/Inc/msb.h
@@ -33,7 +33,8 @@ 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);
+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);

From abfd907d0ecbd924b0b27def97c1d38c27cb1674 Mon Sep 17 00:00:00 2001
From: Myers-Ty <myers.ty@northeastern.edu>
Date: Thu, 5 Dec 2024 21:02:06 -0500
Subject: [PATCH 12/12] bug

---
 Core/Src/msb.c | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index ffcdf9a..3470f5e 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -193,12 +193,10 @@ int32_t imu_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx,
 	if (mut_stat)
 		return mut_stat;
 	HAL_StatusTypeDef hal_stat =
-		lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp);
+		lsm6dso_data_get(ctx, aux_ctx, imu_md_temp, imu_data_temp);
+	osMutexRelease(i2c_mutex);
 	if (hal_stat)
 		return hal_stat;
-
-	// memcpy(gyro, imu.gyro, 3);
-
-	osMutexRelease(i2c_mutex);
+	return 0;
 }
 #endif
\ No newline at end of file