From 0bc13df1a22ab6c9cceb3b7a87b294e604e79c1e Mon Sep 17 00:00:00 2001
From: Pol Henarejos <pol.henarejos@cttc.es>
Date: Mon, 28 Nov 2022 18:54:01 +0100
Subject: [PATCH] Queues are emptied at begining of card thread.

Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>
---
 src/usb/usb.c | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/usb/usb.c b/src/usb/usb.c
index 3417238..cad1d6d 100644
--- a/src/usb/usb.c
+++ b/src/usb/usb.c
@@ -159,6 +159,15 @@ void card_init_core1() {
 size_t finished_data_size = 0;
 
 void card_start(void (*func)(void)) {
+    uint32_t m = 0;
+    while (queue_is_empty(&usb_to_card_q) == false) {
+        if (queue_try_remove(&usb_to_card_q, &m) == false)
+            break;
+    }
+    while (queue_is_empty(&card_to_usb_q) == false) {
+        if (queue_try_remove(&card_to_usb_q, &m) == false)
+            break;
+    }
     multicore_reset_core1();
     multicore_launch_core1(func);
     led_set_blink(BLINK_MOUNTED);