diff --git a/Core/Src/app_threadx.c b/Core/Src/app_threadx.c
index a08cda9..2e1b6ff 100644
--- a/Core/Src/app_threadx.c
+++ b/Core/Src/app_threadx.c
@@ -67,11 +67,11 @@ 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);
+  CATCH_ERROR(threads_init(byte_pool), 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 80bc3e5..ae60217 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -172,7 +172,7 @@ int main(void)
 
   /* Init Functions */
   if (can2_init(&hfdcan2) != U_SUCCESS) {
-    PRINTLN_INFO("Initialize CAN Failed.");
+    PRINTLN_ERROR("Initialize CAN Failed.");
     Error_Handler();
   }
 
diff --git a/Core/Src/u_can.c b/Core/Src/u_can.c
index 8be78c0..f85a4d5 100644
--- a/Core/Src/u_can.c
+++ b/Core/Src/u_can.c
@@ -11,7 +11,7 @@ uint8_t can2_init(FDCAN_HandleTypeDef *hcan) {
     /* Init CAN interface */
     HAL_StatusTypeDef status = can_init(&can2, hcan);
     if(status != HAL_OK) {
-        PRINTLN_INFO("Failed to execute can_init() when initializing can2 (Status: %d/%s).", status, hal_status_toString(status));
+        PRINTLN_ERROR("Failed to execute can_init() when initializing can2 (Status: %d/%s).", status, hal_status_toString(status));
         return U_ERROR;
     }
 
@@ -19,7 +19,7 @@ uint8_t can2_init(FDCAN_HandleTypeDef *hcan) {
     uint16_t standard_ids[] = { CERBERUS_MSG_ID, CERBERUS_MSG_ID };
     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]);
+        PRINTLN_ERROR("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;
     }
 
@@ -27,7 +27,7 @@ uint8_t can2_init(FDCAN_HandleTypeDef *hcan) {
     uint32_t extended_ids[] = { CERBERUS_MSG_ID, CERBERUS_MSG_ID };
     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]);
+        PRINTLN_ERROR("Failed to add extended filter to can2 (Status: %d/%s, ID1: %lu, ID2: %lu).", status, hal_status_toString(status), extended_ids[0], extended_ids[1]);
         return U_ERROR;
     }
 
diff --git a/Core/Src/u_inbox.c b/Core/Src/u_inbox.c
index 84a498c..ebef204 100644
--- a/Core/Src/u_inbox.c
+++ b/Core/Src/u_inbox.c
@@ -8,5 +8,8 @@ void inbox_can(can_msg_t *message) {
             Lightning_Board_Light_Status state = message->data[0];
             set_statemachine(state);
             break;
+        default:
+            PRINTLN_WARNING("Unknown Inbox Message. ID: %lu", message->id);
+            break;
     }
 }
\ No newline at end of file
diff --git a/Core/Src/u_mutexes.c b/Core/Src/u_mutexes.c
index dc90e92..f0af86b 100644
--- a/Core/Src/u_mutexes.c
+++ b/Core/Src/u_mutexes.c
@@ -11,7 +11,7 @@ mutex_t state_machine_mutex = {
 
 uint8_t mutexes_init() {
     if (create_mutex(&state_machine_mutex) != U_SUCCESS) {
-        PRINTLN_INFO("mutexes_init() failed.");
+        PRINTLN_ERROR("mutexes_init() failed.");
         return U_ERROR;
     }
 
diff --git a/Core/Src/u_queues.c b/Core/Src/u_queues.c
index 8ac8a37..c1b9eec 100644
--- a/Core/Src/u_queues.c
+++ b/Core/Src/u_queues.c
@@ -19,12 +19,12 @@ queue_t can_outgoing = {
 
 uint8_t queues_init(TX_BYTE_POOL *byte_pool) {
     if (create_queue(byte_pool, &can_incoming) != U_SUCCESS) {
-        PRINTLN_INFO("CAN Incoming queue creation failed.");
+        PRINTLN_ERROR("CAN Incoming queue creation failed.");
         return U_ERROR;
     }
 
     if (create_queue(byte_pool, &can_outgoing) != U_SUCCESS) {
-        PRINTLN_INFO("CAN Outgoing queue creation failed.");
+        PRINTLN_ERROR("CAN Outgoing queue creation failed.");
         return U_ERROR;
     }
 
diff --git a/Core/Src/u_sensors.c b/Core/Src/u_sensors.c
index 1376ffc..d4306c1 100644
--- a/Core/Src/u_sensors.c
+++ b/Core/Src/u_sensors.c
@@ -34,7 +34,7 @@ static int16_t _float_to_int16(float value) {
  * IMU 
  */
 
- typedef struct {
+typedef struct {
     float x;
     float y;
     float z;
@@ -48,15 +48,15 @@ static int32_t _lsm6dsv_read(void *handle, uint8_t register_address, uint8_t *da
     
     /* Send the register address we're trying to read from. */
     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));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
     
     /* Receive the data. */
     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));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to read from the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
     
@@ -71,15 +71,15 @@ static int32_t _lsm6dsv_write(void *handle, uint8_t register_address, uint8_t *d
     
     /* Send register address. */
     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));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
     
     /* Send data. */
     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));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to write to the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
     
@@ -93,10 +93,10 @@ void _delay(uint32_t delay) {
 uint16_t imu_getAccelerometerData(LSM6DSV_Axes_t *axes) {
     int16_t buf[3];
 
-    int status = lsm6dsv_acceleration_raw_get(&imu, buf);
+    uint8_t status = lsm6dsv_acceleration_raw_get(&imu, buf);
 
-    if(status != 0) {
-        PRINTLN_INFO("ERROR: Failed to call lsm6dsv_acceleration_raw_get() (Status: %d).", status);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: Failed to call lsm6dsv_acceleration_raw_get() (Status: %d).", status);
         return U_ERROR;
     }
 
@@ -110,10 +110,10 @@ uint16_t imu_getAccelerometerData(LSM6DSV_Axes_t *axes) {
 uint16_t imu_getGyroscopeData(LSM6DSV_Axes_t *axes) {
     int16_t buf[3];
 
-    int status = lsm6dsv_angular_rate_raw_get(&imu, buf);
+    uint8_t 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);
+    if (status != 0) {
+        PRINTLN_ERROR("ERROR: Failed to call lsm6dso_angular_rate_raw_get() (Status: %d).", status);
         return U_ERROR;
     }
 
@@ -131,7 +131,7 @@ uint16_t init_imu() {
     imu.handle = &hspi1;
 
     uint8_t id;
-    int status = lsm6dsv_device_id_get(&imu, &id);
+    uint8_t status = lsm6dsv_device_id_get(&imu, &id);
 
     if (status != 0) {
         PRINTLN_ERROR("Failed to call lsm6dsv_device_id_get() (Status: %d).", status);
@@ -300,33 +300,33 @@ static int32_t _lis2mdl_read(void *handle, uint8_t register_address, uint8_t *da
     
     /* Send the register address we're trying to read from. */
     status = HAL_SPI_Transmit((SPI_HandleTypeDef *) handle, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
-    if(status != HAL_OK) {
-        PRINTLN_INFO("ERROR: Failed to send register address to lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to send register address to lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
     
     /* Receive the data. */
     status = HAL_SPI_Receive((SPI_HandleTypeDef *) handle, data, length, HAL_MAX_DELAY);
-    if(status != HAL_OK) {
-        PRINTLN_INFO("ERROR: Failed to read from the lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to read from the lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
     
     return 0;
 }
 
-static int32_t _lis2mdl_write(void *handle, uint8_t register_address, uint8_t *data, uint16_t length){
+static int32_t _lis2mdl_write(void *handle, uint8_t register_address, const uint8_t *data, uint16_t length){
     HAL_StatusTypeDef status;
 
     status = HAL_SPI_Transmit((SPI_HandleTypeDef *)handle, &register_address, sizeof(register_address), HAL_MAX_DELAY);
-    if(status != HAL_OK) {
-        PRINTLN_INFO("ERROR: Failed to send register address to lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to send register address to lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
     
     status = HAL_SPI_Transmit((SPI_HandleTypeDef *)handle, data, length, HAL_MAX_DELAY);
-    if(status != HAL_OK) {
-        PRINTLN_INFO("ERROR: Failed to write to the lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
+    if (status != HAL_OK) {
+        PRINTLN_ERROR("ERROR: Failed to write to the lis2mdl over SPI (Status: %d/%s).", status, hal_status_toString(status));
         return -1;
     }
 
@@ -338,7 +338,7 @@ uint16_t init_magnetometer() {
 
     lis2mdl_ctx = malloc(sizeof(stmdev_ctx_t));
     if (lis2mdl_ctx == NULL) {
-        PRINTLN_INFO("lis2mdl_ctx struct malloc failed.");
+        PRINTLN_ERROR("lis2mdl_ctx struct malloc failed.");
         return U_ERROR;
     }
 
@@ -349,7 +349,7 @@ uint16_t init_magnetometer() {
     lis2mdl_device_id_get(lis2mdl_ctx, &status);
 
     if (status != LIS2MDL_ID) {
-        PRINTLN_INFO("Device ID is not for LIS2MDL (Status %d/%s)", status, hal_status_toString(status));
+        PRINTLN_ERROR("Device ID is not for LIS2MDL (Status %d/%s)", status, hal_status_toString(status));
         return U_ERROR;
     }
 
@@ -387,7 +387,11 @@ uint16_t read_magnetometer() {
     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 = MAGNOMETER_MSG_ID, .len = 6, .data = { 0 } };
+    can_msg_t message = { 
+        .id = MAGNOMETER_MSG_ID, 
+        .len = 6, 
+        .data = { 0 } 
+    };
 
     memcpy(message.data, &axes_data, sizeof(axes_data));
 
diff --git a/Core/Src/u_statemachine.c b/Core/Src/u_statemachine.c
index 5d81fcc..260b7bb 100644
--- a/Core/Src/u_statemachine.c
+++ b/Core/Src/u_statemachine.c
@@ -8,7 +8,7 @@ uint8_t set_statemachine(Lightning_Board_Light_Status state) {
     int status = mutex_get(&state_machine_mutex);
 
     if (status != TX_SUCCESS) {
-        PRINTLN_INFO("ERROR: Failed to get statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
+        PRINTLN_ERROR("ERROR: Failed to get statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
         return U_ERROR;
     }
 
@@ -17,7 +17,7 @@ uint8_t set_statemachine(Lightning_Board_Light_Status state) {
     status = mutex_put(&state_machine_mutex);
 
     if (status != TX_SUCCESS) {
-        PRINTLN_INFO("ERROR: Failed to put statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
+        PRINTLN_ERROR("ERROR: Failed to put statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
         return U_ERROR;
     }
 
@@ -28,7 +28,7 @@ Lightning_Board_Light_Status get_current_state() {
     int status = mutex_get(&state_machine_mutex);
 
     if (status != TX_SUCCESS) {
-        PRINTLN_INFO("ERROR: Failed to get statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
+        PRINTLN_ERROR("ERROR: Failed to get statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
         return LIGHT_OFF;
     }
     
@@ -37,7 +37,7 @@ Lightning_Board_Light_Status get_current_state() {
     status = mutex_put(&state_machine_mutex);
 
     if (status != TX_SUCCESS) {
-        PRINTLN_INFO("ERROR: Failed to put statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
+        PRINTLN_ERROR("ERROR: Failed to put statemachine mutex. (Status: %d/%s).", status, tx_status_toString(status));
         return state;
     }
 
diff --git a/Core/Src/u_test.c b/Core/Src/u_test.c
index d1f74a8..a147228 100644
--- a/Core/Src/u_test.c
+++ b/Core/Src/u_test.c
@@ -3,10 +3,10 @@
 #include "u_test.h"
 
 void gpio_test() {
-    int value = rand() % 3;
-    int status = set_statemachine((Lightning_Board_Light_Status) value);
+    uint8_t value = rand() % 3;
+    uint8_t status = set_statemachine((Lightning_Board_Light_Status) value);
 
     if (status != U_SUCCESS) {
-        PRINTLN_INFO("Failed to set State in GPIO Test");
+        PRINTLN_ERROR("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 fc7d880..fff050f 100644
--- a/Core/Src/u_threads.c
+++ b/Core/Src/u_threads.c
@@ -81,7 +81,7 @@ void can_outgoing_thread(ULONG thread_input) {
         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);
+                PRINTLN_WARNING("WARNING: Failed to send message (on can2) after removing from outgoing queue (Message ID: %ld).", message.id);
             }
         }
 
@@ -105,15 +105,15 @@ void sensors_thread(ULONG thread_input) {
     
     while (1) {
         if (read_lightning_sensor() != U_SUCCESS) {
-            PRINTLN_INFO("Reading & Sending Lightning Sensor Data Failed.");
+            PRINTLN_ERROR("Reading & Sending Lightning Sensor Data Failed.");
         }
 
         if (read_imu() != U_SUCCESS) {
-            PRINTLN_INFO("Reading & Sending IMU Data Failed.");
+            PRINTLN_ERROR("Reading & Sending IMU Data Failed.");
         }
 
         if (read_magnetometer() != U_SUCCESS) {
-            PRINTLN_INFO("Reading & Sending Magnetometer Data Failed.");
+            PRINTLN_ERROR("Reading & Sending Magnetometer Data Failed.");
         }
 
         tx_thread_sleep(_sensors_thread.sleep);
@@ -150,7 +150,7 @@ void gpio_lights_thread(ULONG thread_input) {
                 HAL_GPIO_WritePin(RED_GPIO_Port, RED_Pin, GPIO_PIN_RESET);
                 break;
             default:
-                PRINTLN_INFO("State machine state is not in range %d", state);
+                PRINTLN_WARNING("State machine state is not in range %d", state);
                 break;
         }
 
diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base
index 982c01d..163c750 160000
--- a/Drivers/Embedded-Base
+++ b/Drivers/Embedded-Base
@@ -1 +1 @@
-Subproject commit 982c01d0929c90d4bbd2bff8bb7f6cd946883d4d
+Subproject commit 163c7501a8749d0aac70d53e9256c9c1cbc2eace