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] 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