Rx Queue

Core/Inc/canardRxQueue.h

@@ -0,0 +1,18 @@
+#ifndef CANARD_RX_QUEUE_H
+#define CANARD_RX_QUEUE_H
+
+#include <stdlib.h>
+#include <canard.h>
+
+struct CanardRxQueueItem {
+    struct CanardRxQueueItem* next;
+    CanardCANFrame frame;
+};
+
+extern struct CanardRxQueueItem* rxQueueHead;
+extern struct CanardRxQueueItem* rxQueueTail;
+
+void enqueueRxFrame(CanardCANFrame* rx_frame);
+struct CanardRxQueueItem* dequeueRxFrame();
+
+#endif

Core/Src/canardRxQueue.c

@@ -0,0 +1,47 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <canard.h>
+#include "canardRxQueue.h"
+
+struct CanardRxQueueItem* rxQueueHead = NULL;
+struct CanardRxQueueItem* rxQueueTail = NULL;
+
+void enqueueRxFrame(CanardCANFrame* rx_frame) {
+
+  // dynamically allocate memory
+  struct CanardRxQueueItem* new = (struct CanardRxQueueItem*)malloc(sizeof(struct CanardRxQueueItem));
+
+  if (new == NULL) {
+    printf("CanardRxQueueItem memory allocation failed");
+    return;
+  }
+
+  new->next = NULL;
+  new->frame = *rx_frame;
+
+  if (rxQueueTail) {
+    rxQueueTail->next = new;
+  } else {
+    rxQueueHead = new;
+  }
+
+  rxQueueTail = new;
+}
+
+
+struct CanardRxQueueItem* dequeueRxFrame() {
+
+  if (!rxQueueHead) {
+    return NULL; // queue is empty
+  }
+
+  struct CanardRxQueueItem* dequeuedItem  = rxQueueHead;
+  rxQueueHead = rxQueueHead->next;
+
+  if (!rxQueueHead) {
+    rxQueueTail = NULL; // after dequeuing, the queue is empty
+  }
+
+  return dequeuedItem;
+}
+

Core/Src/main.c

@@ -32,6 +32,7 @@
 #include "../dsdlc_generated/inc/dronecan_msgs.h"
 #include <canard_stm32_driver.h>
 #include "node_settings.h"
+#include "canardRxQueue.h"
 
 /* USER CODE END Includes */
 
@@ -149,17 +150,26 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
 	// Receiving
 	CanardCANFrame rx_frame;
 
-	const uint64_t timestamp = HAL_GetTick() * 1000ULL;
 	const int16_t rx_res = canardSTM32Recieve(hcan, CAN_RX_FIFO0, &rx_frame);
 
 	if (rx_res < 0) {
 		printf("Receive error %d\n", rx_res);
 	}
 	else if (rx_res > 0)        // Success - process the frame
 	{
-		canardHandleRxFrame(&canard, &rx_frame, timestamp);
+		enqueueRxFrame(&rx_frame);
 	}
 }
+
+void processCanardRxQueue() {
+  struct CanardRxQueueItem* currentRxFrame = dequeueRxFrame();
+  const uint64_t timestamp = HAL_GetTick() * 1000ULL;
+
+  canardHandleRxFrame(&canard, &currentRxFrame->frame, timestamp);
+
+  free(currentRxFrame);
+}
+
 // NOTE: All canard handlers and senders are based on this reference: https://dronecan.github.io/Specification/7._List_of_standard_data_types/
 // Alternatively, you can look at the corresponding generated header file in the dsdlc_generated folder
 
@@ -594,6 +604,9 @@ int main(void)
     /* USER CODE BEGIN 3 */
 
 	  processCanardTxQueue(&hcan1);
+
+ 	  processCanardRxQueue();
+
 	  const uint64_t ts = HAL_GetTick();
 
 	  if (ts >= next_1hz_service_at){