More fixes

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

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 */

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 */
 

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

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;
 }

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);