From f1f146c887d476b37fe66e1164f89fea95143e74 Mon Sep 17 00:00:00 2001
From: Daniel Nakhooda <dnakhooda@gmail.com>
Date: Sun, 9 Nov 2025 17:02:38 -0500
Subject: [PATCH 1/4] testing stuff

---
 CMakeLists.txt       |  1 +
 Core/Inc/u_test.h    | 11 +++++++++++
 Core/Src/u_test.c    | 12 ++++++++++++
 Core/Src/u_threads.c |  8 ++++++++
 4 files changed, 32 insertions(+)
 create mode 100644 Core/Inc/u_test.h
 create mode 100644 Core/Src/u_test.c

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ce53ecf..8164f86 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,6 +71,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     "../Drivers/Embedded-Base/general/src/lis2mdl_reg.c"
 
     # Core
+    "./Core/Src/u_test.c"
     "./Core/Src/u_inbox.c"
     "./Core/Src/u_sensors.c"
     "./Core/Src/u_threads.c"
diff --git a/Core/Inc/u_test.h b/Core/Inc/u_test.h
new file mode 100644
index 0000000..eaca6c2
--- /dev/null
+++ b/Core/Inc/u_test.h
@@ -0,0 +1,11 @@
+#ifndef __TEST_H
+#define __TEST_H
+
+#include "u_statemachine.h"
+
+/**
+ * @brief Randomly sets the state of the State Machine
+ */
+void gpio_test();
+
+#endif /* u_test.h */
\ No newline at end of file
diff --git a/Core/Src/u_test.c b/Core/Src/u_test.c
new file mode 100644
index 0000000..d1f74a8
--- /dev/null
+++ b/Core/Src/u_test.c
@@ -0,0 +1,12 @@
+#include <stdlib.h>
+
+#include "u_test.h"
+
+void gpio_test() {
+    int value = rand() % 3;
+    int status = set_statemachine((Lightning_Board_Light_Status) value);
+
+    if (status != U_SUCCESS) {
+        PRINTLN_INFO("Failed to set State in GPIO Test");
+    }
+}
\ No newline at end of file
diff --git a/Core/Src/u_threads.c b/Core/Src/u_threads.c
index 5930a56..69bbb8f 100644
--- a/Core/Src/u_threads.c
+++ b/Core/Src/u_threads.c
@@ -7,6 +7,9 @@
 #include "bitstream.h"
 #include "u_statemachine.h"
 #include "u_mutexes.h"
+#include "u_test.h"
+
+#define TEST_MODE 1
 
 /* Default Thread */
 static thread_t _default_thread = {
@@ -129,6 +132,7 @@ static thread_t _gpio_lights_thread = {
     .function   = gpio_lights_thread         /* Thread Function */
 };
 void gpio_lights_thread(ULONG thread_input) {
+
     while (1) {
         Lightning_Board_Light_Status state = get_current_state();
 
@@ -151,6 +155,10 @@ void gpio_lights_thread(ULONG thread_input) {
         }
 
         tx_thread_sleep(_gpio_lights_thread.sleep);
+
+        #if TEST_MODE
+        gpio_test();
+        #endif
     }
 }
 

From af4ac928b760586b36bf96ac8b9bb5161a9ee85a Mon Sep 17 00:00:00 2001
From: Daniel Nakhooda <dnakhooda@gmail.com>
Date: Sun, 9 Nov 2025 17:06:39 -0500
Subject: [PATCH 2/4] small fix

---
 Core/Src/u_threads.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Core/Src/u_threads.c b/Core/Src/u_threads.c
index 69bbb8f..96b96f4 100644
--- a/Core/Src/u_threads.c
+++ b/Core/Src/u_threads.c
@@ -138,8 +138,8 @@ void gpio_lights_thread(ULONG thread_input) {
 
         switch (state) {
             case LIGHT_GREEN:
-                HAL_GPIO_WritePin(GREEN_GPIO_Port, GREEN_Pin, GPIO_PIN_SET);
                 HAL_GPIO_WritePin(RED_GPIO_Port, RED_Pin, GPIO_PIN_RESET);
+                HAL_GPIO_WritePin(GREEN_GPIO_Port, GREEN_Pin, GPIO_PIN_SET);
                 break;
             case LIGHT_RED:
                 HAL_GPIO_WritePin(GREEN_GPIO_Port, GREEN_Pin, GPIO_PIN_RESET);

From cc9a3788a1f817ae130fc3de1e81241176ba595a Mon Sep 17 00:00:00 2001
From: Daniel Nakhooda <dnakhooda@gmail.com>
Date: Sun, 23 Nov 2025 00:10:31 -0500
Subject: [PATCH 3/4] fix

---
 Core/Inc/u_sensors.h   |  6 +++---
 Core/Src/app_threadx.c |  7 +++++++
 Core/Src/main.c        | 14 +-------------
 Core/Src/u_can.c       |  4 ++--
 Core/Src/u_sensors.c   | 32 ++++++++++++++++----------------
 Core/Src/u_threads.c   |  4 ++--
 Drivers/Embedded-Base  |  2 +-
 7 files changed, 32 insertions(+), 37 deletions(-)

diff --git a/Core/Inc/u_sensors.h b/Core/Inc/u_sensors.h
index 6764b78..1e86e85 100644
--- a/Core/Inc/u_sensors.h
+++ b/Core/Inc/u_sensors.h
@@ -9,7 +9,7 @@
  * @brief initializes IMU for reading
  * @return returns tx status for errors
  */
-uint16_t init_imu(SPI_HandleTypeDef *given_imu_spi);
+uint16_t init_imu();
 
 /**
  * @brief reads and sends over CAN information from the IMU
@@ -21,7 +21,7 @@ uint16_t read_imu();
  * @brief initializes lightning sensor struct used for reading values
  * @return returns tx status for errors
  */
-uint16_t init_lightning_sensor(SPI_HandleTypeDef *hspi);
+uint16_t init_lightning_sensor();
 
 /**
  * @brief reads and sends over CAN information from the lightning sensor
@@ -33,7 +33,7 @@ uint16_t read_lightning_sensor();
  * @brief initializes Magnetometer for reading
  * @return returns tx status for errors
  */
-uint16_t init_magnetometer(SPI_HandleTypeDef *given_compass_spi);
+uint16_t init_magnetometer();
 
 /**
  * @brief reads and returns the information from magnetometer
diff --git a/Core/Src/app_threadx.c b/Core/Src/app_threadx.c
index 3374951..a08cda9 100644
--- a/Core/Src/app_threadx.c
+++ b/Core/Src/app_threadx.c
@@ -24,6 +24,9 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "u_queues.h"
+#include "u_threads.h"
+#include "u_mutexes.h"
+#include "u_sensors.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -65,6 +68,10 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
   /* Init user-written code that uses ThreadX stuff here. */
   CATCH_ERROR(queues_init(byte_pool), U_SUCCESS);
   CATCH_ERROR(threads_init(byte_pool), U_SUCCESS);
+  CATCH_ERROR(mutexes_init(), U_SUCCESS);
+  CATCH_ERROR(init_imu(), U_SUCCESS);
+  CATCH_ERROR(init_lightning_sensor(), U_SUCCESS);
+  CATCH_ERROR(init_magnetometer(), U_SUCCESS);
 
   /* USER CODE END App_ThreadX_MEM_POOL */
   /* USER CODE BEGIN App_ThreadX_Init */
diff --git a/Core/Src/main.c b/Core/Src/main.c
index e5af523..80bc3e5 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -120,7 +120,7 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
 			}
 
 			/* Send message to incoming CAN queue */
-      queue_send(&can_incoming, &message);
+      queue_send(&can_incoming, &message, TX_NO_WAIT);
 		}
 	}
 }
@@ -175,18 +175,6 @@ int main(void)
     PRINTLN_INFO("Initialize CAN Failed.");
     Error_Handler();
   }
-  
-  if (init_imu(&hspi1) != U_SUCCESS) {
-    PRINTLN_INFO("Initialize IMU Failed.");
-  }
-
-  if (init_lightning_sensor(&hspi2) != U_SUCCESS) {
-    PRINTLN_INFO("Initialize Lightning Sensor Failed.");
-  }
-
-  if (init_magnetometer(&hspi3) != U_SUCCESS) {
-    PRINTLN_INFO("Initialize Magnetometer Failed.");
-  }
 
   /* USER CODE END 2 */
 
diff --git a/Core/Src/u_can.c b/Core/Src/u_can.c
index ca261c4..711aef9 100644
--- a/Core/Src/u_can.c
+++ b/Core/Src/u_can.c
@@ -17,14 +17,14 @@ uint8_t can2_init(FDCAN_HandleTypeDef *hcan) {
 
     /* Add filters for standard IDs */
     uint16_t standard_ids[] = { CERBERUS_MSG, CERBERUS_MSG };
-    status = can_add_filter_standard(&can2, &standard_ids);
+    status = can_add_filter_standard(&can2, standard_ids);
     if(status != HAL_OK) {
         PRINTLN_INFO("Failed to add standard filter to can2 (Status: %d/%s, ID1: %d, ID2: %d).", status, hal_status_toString(status), standard_ids[0], standard_ids[1]);
         return U_ERROR;
     }
 
     /* Add filters for extended IDs */
-    uint16_t extended_ids[] = { CERBERUS_MSG, CERBERUS_MSG };
+    uint32_t extended_ids[] = { CERBERUS_MSG, CERBERUS_MSG };
     status = can_add_filter_extended(&can2, extended_ids);
     if (status != HAL_OK) {
         PRINTLN_INFO("Failed to add extended filter to can2 (Status: %d/%s, ID1: %u, ID2: %u).", status, hal_status_toString(status), extended_ids[0], extended_ids[1]);
diff --git a/Core/Src/u_sensors.c b/Core/Src/u_sensors.c
index 827bffd..fa51eec 100644
--- a/Core/Src/u_sensors.c
+++ b/Core/Src/u_sensors.c
@@ -10,14 +10,15 @@
 #include "u_tx_debug.h"
 #include "u_queues.h"
 
-static SPI_HandleTypeDef *imu_spi = NULL;
+extern SPI_HandleTypeDef hspi1; // imu
+extern SPI_HandleTypeDef hspi2; // lightning sensor
+extern SPI_HandleTypeDef hspi3; // magnetometer
 
 static LSM6DSO_Object_t imu;
 static as3935_t *as3935 = NULL;
 static stmdev_ctx_t *lis2mdl_ctx = NULL;
 
 
-
 /** 
  * IMU 
  */
@@ -28,14 +29,14 @@ static int32_t _lsm6dso_read(uint16_t device_address, uint16_t register_address,
     HAL_StatusTypeDef status;
     
     /* Send the register address we're trying to read from. */
-    status = HAL_SPI_Transmit(imu_spi, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
+    status = HAL_SPI_Transmit(&hspi1, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return LSM6DSO_ERROR;
     }
     
     /* Receive the data. */
-    status = HAL_SPI_Receive(imu_spi, data, length, HAL_MAX_DELAY);
+    status = HAL_SPI_Receive(&hspi1, data, length, HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to read from the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return LSM6DSO_ERROR;
@@ -50,14 +51,14 @@ static int32_t _lsm6dso_write(uint16_t device_address, uint16_t register_address
     HAL_StatusTypeDef status;
     
     /* Send register address. */
-    status = HAL_SPI_Transmit(imu_spi, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
+    status = HAL_SPI_Transmit(&hspi1, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return LSM6DSO_ERROR;
     }
     
     /* Send data. */
-    status = HAL_SPI_Transmit(imu_spi, data, length, HAL_MAX_DELAY);
+    status = HAL_SPI_Transmit(&hspi1, data, length, HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to write to the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return LSM6DSO_ERROR;
@@ -96,8 +97,7 @@ uint16_t imu_getGyroscopeData(LSM6DSO_Axes_t *axes) {
  * @brief initializes imu for reading
  * @return returns tx status for errors
  */
-uint16_t init_imu(SPI_HandleTypeDef *given_imu_spi) {
-    imu_spi = given_imu_spi;
+uint16_t init_imu() {
 
     LSM6DSO_IO_t io_config = {
         .BusType = LSM6DSO_SPI_4WIRES_BUS,
@@ -201,8 +201,8 @@ uint16_t read_imu() {
     memcpy(imu_accel_msg.data, &accel_data, sizeof(accel_data));
     memcpy(imu_gyro_msg.data, &gyro_data, sizeof(gyro_data));
 
-    queue_send(&can_outgoing, &imu_accel_msg);
-    queue_send(&can_outgoing, &imu_gyro_msg);
+    queue_send(&can_outgoing, &imu_accel_msg, TX_NO_WAIT);
+    queue_send(&can_outgoing, &imu_gyro_msg, TX_NO_WAIT);
 
     return U_SUCCESS;
 }
@@ -213,14 +213,14 @@ uint16_t read_imu() {
  * LIGHTNING SENSOR 
  */
 
-uint16_t init_lightning_sensor(SPI_HandleTypeDef *hspi) {
+uint16_t init_lightning_sensor() {
     as3935 = malloc(sizeof(as3935_t));
     if (as3935 == NULL) {
         PRINTLN_INFO("as3935 struct malloc failed.");
         return U_ERROR;
     }
 
-    as3935_init(as3935, hspi);
+    as3935_init(as3935, &hspi2);
 
     // calibrate
     as3935_calibrate_RCO(as3935);
@@ -252,7 +252,7 @@ uint16_t read_lightning_sensor() {
 
     memcpy(lightning_message.data, &lightning_data, sizeof(lightning_data));
 
-    queue_send(&can_outgoing, &lightning_message);
+    queue_send(&can_outgoing, &lightning_message, TX_NO_WAIT);
 
     return U_SUCCESS;
 }
@@ -302,7 +302,7 @@ static int32_t _lis2mdl_write(void *handle, uint8_t register_address, uint8_t *d
     return 0;
 }
 
-uint16_t init_magnetometer(SPI_HandleTypeDef *given_magnetometer_spi) {
+uint16_t init_magnetometer() {
     uint8_t status;
 
     lis2mdl_ctx = malloc(sizeof(stmdev_ctx_t));
@@ -311,7 +311,7 @@ uint16_t init_magnetometer(SPI_HandleTypeDef *given_magnetometer_spi) {
         return U_ERROR;
     }
 
-    lis2mdl_ctx->handle = given_magnetometer_spi;
+    lis2mdl_ctx->handle = &hspi3;
     lis2mdl_ctx->read_reg = _lis2mdl_read;
     lis2mdl_ctx->write_reg = _lis2mdl_write;
     
@@ -372,7 +372,7 @@ uint16_t read_magnetometer() {
 
     memcpy(message.data, &axes_data, sizeof(axes_data));
 
-    queue_send(&can_outgoing, &message);
+    queue_send(&can_outgoing, &message, TX_NO_WAIT);
 
     return U_SUCCESS;
 }
\ No newline at end of file
diff --git a/Core/Src/u_threads.c b/Core/Src/u_threads.c
index 96b96f4..fc7d880 100644
--- a/Core/Src/u_threads.c
+++ b/Core/Src/u_threads.c
@@ -50,7 +50,7 @@ void can_incoming_thread(ULONG thread_input) {
         can_msg_t message;
 
         /* Process incoming messages */
-        while(queue_receive(&can_incoming, &message) == U_SUCCESS) {
+        while(queue_receive(&can_incoming, &message, TX_WAIT_FOREVER) == U_SUCCESS) {
             inbox_can(&message);
         }
 
@@ -78,7 +78,7 @@ void can_outgoing_thread(ULONG thread_input) {
         uint8_t status;
 
         /* Process outgoing messages */
-        while(queue_receive(&can_outgoing, &message) == U_SUCCESS) {
+        while(queue_receive(&can_outgoing, &message, TX_WAIT_FOREVER) == U_SUCCESS) {
             status = can_send_msg(&can2, &message);
             if(status != U_SUCCESS) {
                 PRINTLN_INFO("WARNING: Failed to send message (on can2) after removing from outgoing queue (Message ID: %ld).", message.id);
diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base
index 899a8fa..255d73d 160000
--- a/Drivers/Embedded-Base
+++ b/Drivers/Embedded-Base
@@ -1 +1 @@
-Subproject commit 899a8fad512ced09e05feef71b9c321b3ec5fcfd
+Subproject commit 255d73d9941e2228ad3c2687241558146950ec34

From 416b2715bdc62b929db6a012418f7ec9f66fb9e3 Mon Sep 17 00:00:00 2001
From: Daniel Nakhooda <dnakhooda@gmail.com>
Date: Sun, 23 Nov 2025 17:31:47 -0500
Subject: [PATCH 4/4] stuff

---
 CMakeLists.txt        |   4 +-
 Core/Src/u_sensors.c  | 223 +++++++++++++++++++++++++-----------------
 Drivers/Embedded-Base |   2 +-
 3 files changed, 137 insertions(+), 92 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8164f86..0ac53a8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -65,8 +65,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     "./Drivers/Embedded-Base/middleware/src/c_utils.c"
 
     # Peripheral Drivers
-    "../Drivers/Embedded-Base/general/src/lsm6dso_reg.c"
-    "../Drivers/Embedded-Base/general/src/lsm6dso.c"
+    "../Drivers/Embedded-Base/general/include/lsm6dsv_reg.h"
+    "../Drivers/Embedded-Base/general/src/lsm6dsv_reg.c"
     "./Drivers/Embedded-Base/general/src/as3935.c"
     "../Drivers/Embedded-Base/general/src/lis2mdl_reg.c"
 
diff --git a/Core/Src/u_sensors.c b/Core/Src/u_sensors.c
index fa51eec..d9a2198 100644
--- a/Core/Src/u_sensors.c
+++ b/Core/Src/u_sensors.c
@@ -4,7 +4,7 @@
 
 #include "c_utils.h"
 #include "as3935.h"
-#include "lsm6dso.h"
+#include "lsm6dsv_reg.h"
 #include "lis2mdl_reg.h"
 #include "u_can.h"
 #include "u_tx_debug.h"
@@ -14,144 +14,201 @@ extern SPI_HandleTypeDef hspi1; // imu
 extern SPI_HandleTypeDef hspi2; // lightning sensor
 extern SPI_HandleTypeDef hspi3; // magnetometer
 
-static LSM6DSO_Object_t imu;
+static stmdev_ctx_t imu;
 static as3935_t *as3935 = NULL;
 static stmdev_ctx_t *lis2mdl_ctx = NULL;
 
+static int16_t _float_to_int16(float value) {
+    if (value > 32767.0f) {
+        return 32767;
+    }
+
+    if (value < -32768.0f) {
+        return -32768;
+    }
+
+    return (int16_t) value;
+}
 
 /** 
  * IMU 
  */
 
-static int32_t _lsm6dso_read(uint16_t device_address, uint16_t register_address, uint8_t *data, uint16_t length) {
+ typedef struct {
+    float x;
+    float y;
+    float z;
+} LSM6DSV_Axes_t;
+
+static int32_t _lsm6dsv_read(void *handle, uint8_t register_address, uint8_t *data, uint16_t length) {
     /* For SPI reads, set MSB = 1 for read operation. */
     uint8_t spi_reg = (uint8_t)(register_address | 0x80);
     HAL_StatusTypeDef status;
+    SPI_HandleTypeDef *spi_handle = (SPI_HandleTypeDef *) handle;
     
     /* Send the register address we're trying to read from. */
-    status = HAL_SPI_Transmit(&hspi1, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
+    status = HAL_SPI_Transmit(spi_handle, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
-        return LSM6DSO_ERROR;
+        return -1;
     }
     
     /* Receive the data. */
-    status = HAL_SPI_Receive(&hspi1, data, length, HAL_MAX_DELAY);
+    status = HAL_SPI_Receive(spi_handle, data, length, HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to read from the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
-        return LSM6DSO_ERROR;
+        return -1;
     }
     
-    return LSM6DSO_OK;
+    return 0;
 }
 
-static int32_t _lsm6dso_write(uint16_t device_address, uint16_t register_address, uint8_t *data, uint16_t length) {
+static int32_t _lsm6dsv_write(void *handle, uint8_t register_address, uint8_t *data, uint16_t length) {
     /* For SPI writes, clear MSB = 0 for write operation. */
     uint8_t spi_reg = (uint8_t)(register_address & 0x7F);
     HAL_StatusTypeDef status;
+    SPI_HandleTypeDef *spi_handle = (SPI_HandleTypeDef *) handle;
     
     /* Send register address. */
-    status = HAL_SPI_Transmit(&hspi1, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
+    status = HAL_SPI_Transmit(spi_handle, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
-        return LSM6DSO_ERROR;
+        return -1;
     }
     
     /* Send data. */
-    status = HAL_SPI_Transmit(&hspi1, data, length, HAL_MAX_DELAY);
+    status = HAL_SPI_Transmit(spi_handle, data, length, HAL_MAX_DELAY);
     if(status != HAL_OK) {
         PRINTLN_INFO("ERROR: Failed to write to the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
-        return LSM6DSO_ERROR;
+        return -1;
     }
     
-    return LSM6DSO_OK;
-}
-
-int32_t _get_tick(void) {
-    return (int32_t)HAL_GetTick();
+    return 0;
 }
 
 void _delay(uint32_t delay) {
     return HAL_Delay(delay);
 }
 
-uint16_t imu_getAccelerometerData(LSM6DSO_Axes_t *axes) {
-    int status = LSM6DSO_ACC_GetAxes(&imu, axes);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to call LSM6DSO_ACC_GetAxes() (Status: %d).", status);
+uint16_t imu_getAccelerometerData(LSM6DSV_Axes_t *axes) {
+    int16_t buf[3];
+
+    int status = lsm6dsv_acceleration_raw_get(&imu, buf);
+
+    if(status != 0) {
+        PRINTLN_INFO("ERROR: Failed to call lsm6dsv_acceleration_raw_get() (Status: %d).", status);
         return U_ERROR;
     }
+
+    axes->x = lsm6dsv_from_fs2_to_mg(buf[0]);
+    axes->y = lsm6dsv_from_fs2_to_mg(buf[1]);
+    axes->z = lsm6dsv_from_fs2_to_mg(buf[2]);
+
     return U_SUCCESS;
 }
 
-uint16_t imu_getGyroscopeData(LSM6DSO_Axes_t *axes) {
-    int status = LSM6DSO_GYRO_GetAxes(&imu, axes);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to call LSM6DSO_GYRO_GetAxes() (Status: %d).", status);
+uint16_t imu_getGyroscopeData(LSM6DSV_Axes_t *axes) {
+    int16_t buf[3];
+
+    int status = lsm6dsv_angular_rate_raw_get(&imu, buf);
+
+    if(status != 0) {
+        PRINTLN_INFO("ERROR: Failed to call lsm6dso_angular_rate_raw_get() (Status: %d).", status);
         return U_ERROR;
     }
+
+    axes->x = lsm6dsv_from_fs250_to_mdps(buf[0]);
+    axes->y = lsm6dsv_from_fs250_to_mdps(buf[1]);
+    axes->z = lsm6dsv_from_fs250_to_mdps(buf[2]);
+
     return U_SUCCESS;
 }
 
-/**
- * @brief initializes imu for reading
- * @return returns tx status for errors
- */
 uint16_t init_imu() {
+    uint8_t whoami;
 
-    LSM6DSO_IO_t io_config = {
-        .BusType = LSM6DSO_SPI_4WIRES_BUS,
-        .WriteReg = _lsm6dso_write,
-        .ReadReg = _lsm6dso_read,
-        .GetTick = _get_tick,
-        .Delay = _delay
-    };
-
-    int status = LSM6DSO_RegisterBusIO(&imu, &io_config);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to call LSM6DSO_RegisterBusIO (Status: %d).", status);
+    imu.read_reg = _lsm6dsv_read;
+    imu.read_reg = _lsm6dsv_write;
+    imu.mdelay = _delay;
+    imu.handle = &hspi1;
+
+    int status = lis2mdl_device_id_get(&imu, &whoami);
+    if(status != 0) {
+        PRINTLN_ERROR("ERROR: Failed to call lis2mdl_device_id_get (Status: %d).", status);
+        return U_ERROR;
+    }
+
+    if (whoami != LIS2MDL_ID) {
+        PRINTLN_ERROR("ERROR: Failed whoami (Status: %d).", status);
+        return U_ERROR;
+    }
+
+    status = lsm6dsv_sw_reset(&imu);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_sw_reset (Status: %d).", status);
+        return U_ERROR;
+    }
+
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_xl_mode_set(&imu, LSM6DSV_XL_HIGH_PERFORMANCE_MD);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_mode_set (Status: %d).", status);
+        return U_ERROR;
+    }
+
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_xl_data_rate_set(&imu, LSM6DSV_EIS_960Hz);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_data_rate_set (Status: %d).", status);
+        return U_ERROR;
+    }
+
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_xl_data_rate_set(&imu, LSM6DSV_EIS_960Hz);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_data_rate_set (Status: %d).", status);
         return U_ERROR;
     }
 
-    /* Initialize IMU. */
-    status = LSM6DSO_Init(&imu);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to run LSM6DS0_Init() (Status: %d).", status);
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_xl_full_scale_set(&imu, LSM6DSV_OIS_2g);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_full_scale_set (Status: %d).", status);
         return U_ERROR;
     }
 
-    /* Setup IMU Accelerometer - default 104Hz */
-	status = LSM6DSO_ACC_SetOutputDataRate_With_Mode(&imu, 833.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to run LSM6DSO_ACC_SetOutputDataRate_With_Mode (Status: %d).", status);
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_filt_xl_lp2_set(&imu, 1);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_filt_xl_lp2_set (Status: %d).", status);
         return U_ERROR;
     }
 
-    /* Enable Accelerometer. */
-    status = LSM6DSO_ACC_Enable(&imu);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to run LSM6DSO_ACC_Enable() (Status: %d).", status);
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_filt_xl_lp2_bandwidth_set(&imu, LSM6DSV_OIS_XL_LP_ULTRA_LIGHT);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_filt_xl_lp2_bandwidth_set (Status: %d).", status);
         return U_ERROR;
     }
-	
-    /* Set Accelerometer Filter Mode. */
-	status = LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 3); // 3 = 'LSM6DSO_LP_ODR_DIV_45'
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to run LSM6DSO_ACC_Set_Filter_Mode() (Status: %d).", status);
+
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_gy_mode_set(&imu, LSM6DSV_GY_HIGH_PERFORMANCE_MD);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_gy_mode_set (Status: %d).", status);
         return U_ERROR;
     }
 
-	/* Setup IMU Gyroscope */
-	status = LSM6DSO_GYRO_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to run LSM6DSO_GYRO_SetOutputDataRate_With_Mode() (Status: %d).", status);
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_gy_data_rate_set(&imu, LSM6DSV_ODR_AT_120Hz);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_gy_data_rate_set (Status: %d).", status);
         return U_ERROR;
     }
 
-    /* Enable IMU Gyroscope. */
-	status = LSM6DSO_GYRO_Enable(&imu);
-    if(status != LSM6DSO_OK) {
-        PRINTLN_INFO("ERROR: Failed to run LSM6DSO_GYRO_Enable() (Status: %d).", status);
+    /** TODO: just picked one that sounds good. double check this */
+    status = lsm6dsv_gy_full_scale_set(&imu, LSM6DSV_250dps);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: failed lsm6dsv_gy_full_scale_set (Status: %d).", status);
         return U_ERROR;
     }
 
@@ -159,8 +216,8 @@ uint16_t init_imu() {
 }
 
 uint16_t read_imu() {
-    LSM6DSO_Axes_t accel_axes;
-    LSM6DSO_Axes_t gyro_axes;
+    LSM6DSV_Axes_t accel_axes;
+    LSM6DSV_Axes_t gyro_axes;
 
     if (imu_getAccelerometerData(&accel_axes) != U_SUCCESS) {
         return U_ERROR;
@@ -176,9 +233,9 @@ uint16_t read_imu() {
 		int16_t accel_z;
 	} accel_data;
 
-    accel_data.accel_x = accel_axes.x;
-    accel_data.accel_y = accel_axes.y;
-    accel_data.accel_z = accel_axes.z;
+    accel_data.accel_x = _float_to_int16(accel_axes.x * 1000);
+    accel_data.accel_y = _float_to_int16(accel_axes.y * 1000);
+    accel_data.accel_z = _float_to_int16(accel_axes.z * 1000);
 
     struct __attribute__((__packed__)) {
 		int16_t gyro_x;
@@ -186,9 +243,9 @@ uint16_t read_imu() {
 		int16_t gyro_z;
 	} gyro_data;
 
-    gyro_data.gyro_x = gyro_axes.x;
-    gyro_data.gyro_y = gyro_axes.y;
-    gyro_data.gyro_z = gyro_axes.z;
+    gyro_data.gyro_x = _float_to_int16(gyro_axes.x * 1000);
+    gyro_data.gyro_y = _float_to_int16(gyro_axes.y * 1000);
+    gyro_data.gyro_z = _float_to_int16(gyro_axes.z * 1000);
 
     can_msg_t imu_accel_msg = { .id = 0xAA,
 				    .len = 6,
@@ -331,18 +388,6 @@ uint16_t init_magnetometer() {
     return U_SUCCESS;
 }
 
-static int16_t float_to_int16_saturate(float value) {
-    if (value > 32767.0f) {
-        return 32767;
-    }
-
-    if (value < -32768.0f) {
-        return -32768;
-    }
-
-    return (int16_t) value;
-}
-
 uint16_t read_magnetometer() {
     if (lis2mdl_ctx == NULL) {
         return U_ERROR;
@@ -364,9 +409,9 @@ uint16_t read_magnetometer() {
 		int16_t axes_3;
 	} axes_data;
 
-    axes_data.axes_1 = float_to_int16_saturate(lis2mdl_from_lsb_to_mgauss(raw_axes[0]) * 1000.0f);
-    axes_data.axes_2 = float_to_int16_saturate(lis2mdl_from_lsb_to_mgauss(raw_axes[1]) * 1000.0f);
-    axes_data.axes_3 = float_to_int16_saturate(lis2mdl_from_lsb_to_mgauss(raw_axes[2]) * 1000.0f);
+    axes_data.axes_1 = _float_to_int16(lis2mdl_from_lsb_to_mgauss(raw_axes[0]) * 1000.0f);
+    axes_data.axes_2 = _float_to_int16(lis2mdl_from_lsb_to_mgauss(raw_axes[1]) * 1000.0f);
+    axes_data.axes_3 = _float_to_int16(lis2mdl_from_lsb_to_mgauss(raw_axes[2]) * 1000.0f);
 
     can_msg_t message = { .id = 0xAD, .len = 6, .data = { 0 } };
 
diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base
index 255d73d..60c135a 160000
--- a/Drivers/Embedded-Base
+++ b/Drivers/Embedded-Base
@@ -1 +1 @@
-Subproject commit 255d73d9941e2228ad3c2687241558146950ec34
+Subproject commit 60c135a9c4945f0e239597fb3964ae0bc73cd785