Fixes

Core/Inc/u_can.h

@@ -14,7 +14,11 @@ uint8_t can2_init(FDCAN_HandleTypeDef *hcan);
 extern can_t can2;
 
 /* List of CAN IDs */
-#define CERBERUS_MSG  0xCA
+#define CERBERUS_MSG_ID          0x0CA
+#define IMU_ACCEL_MSG_ID         0xAAA
+#define IMU_GYRO_MSG_ID          0xAAB
+#define LIGHTNING_SENSOR_MSG_ID  0xAAC
+#define MAGNOMETER_MSG_ID        0xAAD
 
 
 #endif /* u_can.h */

Core/Src/u_can.c

@@ -16,15 +16,15 @@ uint8_t can2_init(FDCAN_HandleTypeDef *hcan) {
     }
 
     /* Add filters for standard IDs */
-    uint16_t standard_ids[] = { CERBERUS_MSG, CERBERUS_MSG };
+    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]);
         return U_ERROR;
     }
 
     /* Add filters for extended IDs */
-    uint32_t extended_ids[] = { CERBERUS_MSG, CERBERUS_MSG };
+    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]);

Core/Src/u_inbox.c

@@ -4,7 +4,7 @@
 
 void inbox_can(can_msg_t *message) {
     switch(message->id) {
-        case CERBERUS_MSG:
+        case CERBERUS_MSG_ID:
             Lightning_Board_Light_Status state = message->data[0];
             set_statemachine(state);
             break;

Core/Src/u_queues.c

@@ -3,17 +3,6 @@
 #include "u_tx_debug.h"
 #include <stdio.h>
 
-/* 
-*  NOTE: This file is kinda weird because of how ThreadX queues work. The size of each message in a ThreadX queue has to be a multiple of 4 bytes, since ThreadX
-*  queues are implemented as arrays of 32-bit words. So, to create queues for structs that aren't a multiple of 4 bytes, you have to round up to the nearest multiple of 4 bytes.
-*  This is handled in the _create_queue() function below.
-*
-*  Then, the queue_send() and queue_receive() functions use a buffer to send/receive messages to/from the queue.
-*  This handles any conversions between the actual message size and the rounded-up size.
-*
-*  On the bright side (assuming my code works), this file should automatically handle all the 32-bit word stuff for you so you don't have to worry about it.
-*/
-
 /* Incoming CAN Queue */
 queue_t can_incoming = {
     .name = "Incoming CAN Queue",           /* Name of the queue. */

Core/Src/u_sensors.c

@@ -125,90 +125,64 @@ uint16_t imu_getGyroscopeData(LSM6DSV_Axes_t *axes) {
 }
 
 uint16_t init_imu() {
-    uint8_t whoami;
-
     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;
-    }
+    uint8_t id;
+    int status = lsm6dsv_device_id_get(&imu, &id);
 
-    /** 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);
+        PRINTLN_ERROR("Failed to call lsm6dsv_device_id_get() (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);
+    if (id != LSM6DSV_ID) {
+        PRINTLN_ERROR("lsm6dsv_device_id_get() returned an unexpected ID (id=%d, expected=%d). This means that the IMU is not configured correctly.", id, LSM6DSV_ID);
         return U_ERROR;
     }
 
-    /** TODO: just picked one that sounds good. double check this */
-    status = lsm6dsv_xl_data_rate_set(&imu, LSM6DSV_EIS_960Hz);
+    /* Reset IMU. */
+    status = lsm6dsv_reset_set(&imu, LSM6DSV_GLOBAL_RST);
     if (status != 0) {
-        PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_data_rate_set (Status: %d).", status);
+        PRINTLN_ERROR("Failed to reset the IMU via lsm6dsv_reset_set() (Status: %d).", status);
         return U_ERROR;
     }
+    HAL_Delay(30); // This is probably overkill, but the datasheet lists the gyroscope's "Turn-on time" as 30ms, and I can't find anything else that specifies how long resets take.
 
-    /** TODO: just picked one that sounds good. double check this */
-    status = lsm6dsv_xl_full_scale_set(&imu, LSM6DSV_OIS_2g);
+    /* Enable Block Data Update. */
+    status = lsm6dsv_block_data_update_set(&imu, PROPERTY_ENABLE); // Makes it so "output registers are not updated until LSB and MSB have been read". Datasheet says this is enabled by default but figured it was better to be explicit.
     if (status != 0) {
-        PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_full_scale_set (Status: %d).", status);
+        PRINTLN_ERROR("Failed to enable Block Data Update via lsm6dsv_block_data_update_set() (Status: %d).", status);
         return U_ERROR;
     }
 
-    /** TODO: just picked one that sounds good. double check this */
-    status = lsm6dsv_filt_xl_lp2_set(&imu, 1);
+    /* Set Accelerometer Full Scale. */
+    status = lsm6dsv_xl_full_scale_set(&imu, LSM6DSV_2g);
     if (status != 0) {
-        PRINTLN_ERROR("ERROR: failed lsm6dsv_filt_xl_lp2_set (Status: %d).", status);
+        PRINTLN_ERROR("Failed to set IMU Accelerometer Full Scale via lsm6dsv_xl_full_scale_set() (Status: %d).", status);
         return U_ERROR;
     }
 
-    /** TODO: just picked one that sounds good. double check this */
-    status = lsm6dsv_filt_xl_lp2_bandwidth_set(&imu, LSM6DSV_OIS_XL_LP_ULTRA_LIGHT);
+    /* Set gyroscope full scale. */
+    status = lsm6dsv_gy_full_scale_set(&imu, LSM6DSV_2000dps);
     if (status != 0) {
-        PRINTLN_ERROR("ERROR: failed lsm6dsv_filt_xl_lp2_bandwidth_set (Status: %d).", status);
+        PRINTLN_ERROR("Failed to set IMU Gyroscope Full Scale via lsm6dsv_gy_full_scale_set() (Status: %d).", status);
         return U_ERROR;
     }
 
-    /** TODO: just picked one that sounds good. double check this */
-    status = lsm6dsv_gy_mode_set(&imu, LSM6DSV_GY_HIGH_PERFORMANCE_MD);
+    /* Set accelerometer output data rate. */
+    status = lsm6dsv_xl_data_rate_set(&imu, LSM6DSV_ODR_AT_120Hz);
     if (status != 0) {
-        PRINTLN_ERROR("ERROR: failed lsm6dsv_gy_mode_set (Status: %d).", status);
+        PRINTLN_ERROR("Failed to set IMU Accelerometer Datarate via lsm6dsv_xl_data_rate_set() (Status: %d).", status);
         return U_ERROR;
     }
 
-    /** TODO: just picked one that sounds good. double check this */
+    /* Set gyroscope output data rate. */
     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;
-    }
-
-    /** 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);
+        PRINTLN_ERROR("Failed to set IMU Gyroscope Datarate via lsm6dsv_gy_data_rate_set() (Status: %d).", status);
         return U_ERROR;
     }
 
@@ -247,11 +221,11 @@ uint16_t read_imu() {
     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,
+    can_msg_t imu_accel_msg = { .id = IMU_ACCEL_MSG_ID,
 				    .len = 6,
 				    .data = { 0 } };
 
-	can_msg_t imu_gyro_msg = { .id = 0xAB,
+	can_msg_t imu_gyro_msg = { .id = IMU_GYRO_MSG_ID,
 				   .len = 6,
 				   .data = { 0 } };
 
@@ -295,7 +269,7 @@ uint16_t read_lightning_sensor() {
 
     uint8_t interrupt = as3935_get_interrupt(as3935);
 
-    can_msg_t lightning_message = { .id = 0xAC, .len = 8, .data = { 0 } };
+    can_msg_t lightning_message = { .id = LIGHTNING_SENSOR_MSG_ID, .len = 8, .data = { 0 } };
 
     struct __attribute__((__packed__)) {
 		uint8_t interrupt;
@@ -413,7 +387,7 @@ 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 = 0xAD, .len = 6, .data = { 0 } };
+    can_msg_t message = { .id = MAGNOMETER_MSG_ID, .len = 6, .data = { 0 } };
 
     memcpy(message.data, &axes_data, sizeof(axes_data));