WATonomous · RamyWahib · Apr 1, 2026
diff --git a/embedded/STM32/app/src/firmware.c b/embedded/STM32/app/src/firmware.c
@@ -4,42 +4,63 @@
 #include "FreeRTOS.h"
 #include "stm32g4xx.h"
 #include "task.h"
+#include "queue.h"
+QueueHandle_t can_rx_queue;
 
 #define BOOTLOADER_SIZE (0x08008000U)
 
 // set the vector table offset of app firmware image
 static void vector_setup(void) { SCB->VTOR = BOOTLOADER_SIZE; }
 
 // thread for blinking led
-void blink_led(void *pvParams) {
-  while (1) {
-    HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_5);
-    // vTaskDelay(pdMS_TO_TICKS(500));
-    HAL_Delay(500);
-  }
+void blink_led(void *pvParams)
+{
+    while (1)
+    {
+        HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_5);
+        // vTaskDelay(pdMS_TO_TICKS(500));
+        HAL_Delay(500);
+    }
+}
+void can_rx_task(void *pvParams)
+{
+    CAN_RxMessage_t msg;
+    while (1)
+    {
+        if (xQueueReceive(can_rx_queue, &msg, portMAX_DELAY) == pdTRUE)
+        {
+            printf("ID: 0x%lX | Data: ", msg.header.Identifier);
+            for (int i = 0; i < (int)(msg.header.DataLength >> 16); i++)
+                printf("%02X ", msg.data[i]);
+            printf("\n");
+        }
+    }
 }
 
-int main() {
-  vector_setup();
-  HAL_Init();
-  system_setup();
+int main()
+{
+    vector_setup();
+    HAL_Init();
+    system_setup();
 
-  __HAL_RCC_GPIOA_CLK_ENABLE();
+    __HAL_RCC_GPIOA_CLK_ENABLE();
 
-  // Configure GPIO PIN5;
-  GPIO_InitTypeDef GPIO_InitStruct;
-  GPIO_InitStruct.Pin = GPIO_PIN_5;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+    // Configure GPIO PIN5;
+    GPIO_InitTypeDef GPIO_InitStruct;
+    GPIO_InitStruct.Pin = GPIO_PIN_5;
+    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
-  // create blinking led task
-  xTaskCreate(blink_led, "BLINK_LED", 128, NULL, 1, NULL);
+    // create blinking led task
+    can_rx_queue = xQueueCreate(10, sizeof(CAN_RxMessage_t));
+    xTaskCreate(blink_led, "BLINK_LED", 128, NULL, 1, NULL);
+    xTaskCreate(can_rx_task, "CAN_RX", 256, NULL, 2, NULL);
 
-  // start FreeRTOS Scheduler
-  vTaskStartScheduler();
+    // start FreeRTOS Scheduler
+    vTaskStartScheduler();
 
-  while (1)
-    ;
+    while (1)
+        ;
 }
diff --git a/embedded/STM32/src/Drivers/FDCAN_STM32.cpp → embedded/STM32/src/Drivers/FDCAN_STM32.c b/embedded/STM32/src/Drivers/FDCAN_STM32.cpp → embedded/STM32/src/Drivers/FDCAN_STM32.c
@@ -1,28 +1,37 @@
 /* USER CODE BEGIN PFP */
 
 #include "FDCAN_STM32.h"
-#include <SimpleFOC.h>
+#include <string.h>
+#include "stm32g4xx_hal.h"
+#include "FreeRTOS.h"
+#include "queue.h"
 FDCAN_HandleTypeDef hfdcan2;
 
-static void check_can_bus(FDCAN_HandleTypeDef *hfdcan) {
+static void check_can_bus(FDCAN_HandleTypeDef *hfdcan)
+{
   FDCAN_ProtocolStatusTypeDef protocolStatus = {};
 
   HAL_FDCAN_GetProtocolStatus(hfdcan, &protocolStatus);
-  if (protocolStatus.BusOff) {
+  if (protocolStatus.BusOff)
+  {
     CLEAR_BIT(hfdcan->Instance->CCCR, FDCAN_CCCR_INIT);
   }
 }
 
 void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef *hfdcan,
-                                   uint32_t ErrorStatusITs) {
-  if (hfdcan == &hfdcan2) {
-    if ((ErrorStatusITs & FDCAN_IT_BUS_OFF) != RESET) {
+                                   uint32_t ErrorStatusITs)
+{
+  if (hfdcan == &hfdcan2)
+  {
+    if ((ErrorStatusITs & FDCAN_IT_BUS_OFF) != RESET)
+    {
       check_can_bus(hfdcan);
     }
   }
 }
 
-void MX_FDCAN2_Init(void) {
+void MX_FDCAN2_Init(void)
+{
 
   /* USER CODE BEGIN FDCAN2_Init 0 */
 
@@ -49,7 +58,8 @@ void MX_FDCAN2_Init(void) {
   hfdcan2.Init.StdFiltersNbr = 1;
   hfdcan2.Init.ExtFiltersNbr = 0;
   hfdcan2.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
-  if (HAL_FDCAN_Init(&hfdcan2) != HAL_OK) {
+  if (HAL_FDCAN_Init(&hfdcan2) != HAL_OK)
+  {
     Error_Handler();
   }
   /* USER CODE BEGIN FDCAN2_Init 2 */
@@ -59,11 +69,13 @@ void MX_FDCAN2_Init(void) {
 
 static uint32_t HAL_RCC_FDCAN_CLK_ENABLED = 0;
 
-void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *fdcanHandle) {
+void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *fdcanHandle)
+{
 
   GPIO_InitTypeDef GPIO_InitStruct = {0};
   RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
-  if (fdcanHandle->Instance == FDCAN2) {
+  if (fdcanHandle->Instance == FDCAN2)
+  {
     /* USER CODE BEGIN FDCAN2_MspInit 0 */
 
     /* USER CODE END FDCAN2_MspInit 0 */
@@ -72,13 +84,15 @@ void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *fdcanHandle) {
      */
     PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_FDCAN;
     PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL;
-    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) {
+    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
+    {
       Error_Handler();
     }
 
     /* FDCAN2 clock enable */
     HAL_RCC_FDCAN_CLK_ENABLED++;
-    if (HAL_RCC_FDCAN_CLK_ENABLED == 1) {
+    if (HAL_RCC_FDCAN_CLK_ENABLED == 1)
+    {
       __HAL_RCC_FDCAN_CLK_ENABLE();
     }
 
@@ -105,15 +119,18 @@ void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *fdcanHandle) {
   }
 }
 
-void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef *fdcanHandle) {
+void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef *fdcanHandle)
+{
 
-  if (fdcanHandle->Instance == FDCAN2) {
+  if (fdcanHandle->Instance == FDCAN2)
+  {
     /* USER CODE BEGIN FDCAN2_MspDeInit 0 */
 
     /* USER CODE END FDCAN2_MspDeInit 0 */
     /* Peripheral clock disable */
     HAL_RCC_FDCAN_CLK_ENABLED--;
-    if (HAL_RCC_FDCAN_CLK_ENABLED == 0) {
+    if (HAL_RCC_FDCAN_CLK_ENABLED == 0)
+    {
       __HAL_RCC_FDCAN_CLK_DISABLE();
     }
 
@@ -131,3 +148,36 @@ void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef *fdcanHandle) {
     /* USER CODE END FDCAN2_MspDeInit 1 */
   }
 }
+
+void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
+{
+
+  // check if rx interrupt line was set
+  if (RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE)
+  {
+    FDCAN_RxHeaderTypeDef rxHeader;
+    uint8_t rxData[8];
+
+    // check rx fifo level
+    while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) > 0)
+    {
+
+      HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rxHeader, rxData);
+      CAN_RxMessage_t msg;
+      msg.header = rxHeader;
+      memcpy(msg.data, rxData, 8);
+      BaseType_t higher_priority_task_woken = pdFALSE;
+      xQueueSendFromISR(can_rx_queue, &msg, &higher_priority_task_woken);
+      portYIELD_FROM_ISR(higher_priority_task_woken);
+    }
+  }
+}
+
+/*
+
+Build callback function for whenever a message is recieved via RXFifo0
+check amount of data in FIFO
+if(fifo > 0) -> read the message
+push message into queue (build queue in firmware.c)
+
+*/
diff --git a/embedded/STM32/src/Drivers/FDCAN_STM32.h b/embedded/STM32/src/Drivers/FDCAN_STM32.h
@@ -1,17 +1,24 @@
 #include <stm32g4xx_hal_fdcan.h>
 
 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif
-void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef *hfdcan,
-                                   uint32_t ErrorStatusITs);
-void MX_FDCAN2_Init(void);
-void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *fdcanHandle);
-void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef *fdcanHandle);
-extern FDCAN_HandleTypeDef hfdcan2;
+    void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef *hfdcan,
+                                       uint32_t ErrorStatusITs);
+    void MX_FDCAN2_Init(void);
+    void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *fdcanHandle);
+    void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef *fdcanHandle);
+
+    typedef struct
+    {
+        FDCAN_RxHeaderTypeDef header;
+        uint8_t data[8];
+    } CAN_RxMessage_t;
+    extern FDCAN_HandleTypeDef hfdcan2;
+    extern QueueHandle_t can_rx_queue;
 
 #ifdef __cplusplus
 }
 #endif
 
-static void check_can_bus(FDCAN_HandleTypeDef *hfdcan);