diff --git a/config/freertos/crazyflie21/board.repos b/config/freertos/crazyflie21/board.repos
index f451d48f..bf97dca8 100644
--- a/config/freertos/crazyflie21/board.repos
+++ b/config/freertos/crazyflie21/board.repos
@@ -2,7 +2,7 @@ repositories:
   crazyflie_firmware:
     type: git
     url: https://github.com/bitcraze/crazyflie-firmware
-    version: 2020.06
+    version: 2021.06
 
   freertos_apps:
     type: git
diff --git a/config/freertos/crazyflie21/create.sh b/config/freertos/crazyflie21/create.sh
index fb126d85..4d030a62 100755
--- a/config/freertos/crazyflie21/create.sh
+++ b/config/freertos/crazyflie21/create.sh
@@ -10,6 +10,7 @@ pushd $FW_TARGETDIR >/dev/null
 
     # Import repos
     vcs import --input $PREFIX/config/$RTOS/$PLATFORM/board.repos
+    git apply $PREFIX/config/$RTOS/$PLATFORM/patchs/*.patch --directory crazyflie_firmware
 
     # ignore broken packages
     touch mcu_ws/ros2/rcl_logging/rcl_logging_log4cxx/COLCON_IGNORE
diff --git a/config/freertos/crazyflie21/patchs/0001-Add-tx-queue-utilities.patch b/config/freertos/crazyflie21/patchs/0001-Add-tx-queue-utilities.patch
new file mode 100644
index 00000000..3e62be1f
--- /dev/null
+++ b/config/freertos/crazyflie21/patchs/0001-Add-tx-queue-utilities.patch
@@ -0,0 +1,47 @@
+From ab2fc76ed60e64f52cb6379ffdf4a04691af658d Mon Sep 17 00:00:00 2001
+From: Antonio cuadros <acuadros1995@gmail.com>
+Date: Mon, 27 Sep 2021 09:21:33 +0200
+Subject: [PATCH] Add tx queue utilities
+
+---
+ src/hal/interface/radiolink.h |  3 ++-
+ src/hal/src/radiolink.c       | 10 ++++++++++
+ 2 files changed, 12 insertions(+), 1 deletion(-)
+
+diff --git a/src/hal/interface/radiolink.h b/src/hal/interface/radiolink.h
+index 293c5c64..ad85e313 100644
+--- a/src/hal/interface/radiolink.h
++++ b/src/hal/interface/radiolink.h
+@@ -58,6 +58,7 @@ void radiolinkSyslinkDispatch(SyslinkPacket *slp);
+ struct crtpLinkOperations * radiolinkGetLink();
+ bool radiolinkSendP2PPacketBroadcast(P2PPacket *p2pp);
+ void p2pRegisterCB(P2PCallback cb);
+-
++int linkGetFreeTxQueuePackets(void);
++int linkResetTxQueuePackets(void);
+ 
+ #endif //__RADIO_H__
+diff --git a/src/hal/src/radiolink.c b/src/hal/src/radiolink.c
+index 738901f3..23d000b3 100644
+--- a/src/hal/src/radiolink.c
++++ b/src/hal/src/radiolink.c
+@@ -74,6 +74,16 @@ static bool radiolinkIsConnected(void) {
+   return (xTaskGetTickCount() - lastPacketTick) < M2T(RADIO_ACTIVITY_TIMEOUT_MS);
+ }
+ 
++int linkGetFreeTxQueuePackets(void)
++{
++  return (RADIOLINK_TX_QUEUE_SIZE - uxQueueMessagesWaiting(txQueue));
++}
++
++int linkResetTxQueuePackets(void)
++{
++  return xQueueReset(txQueue);
++}
++
+ static struct crtpLinkOperations radiolinkOp =
+ {
+   .setEnable         = radiolinkSetEnable,
+-- 
+2.25.1
+
diff --git a/config/freertos/crazyflie21/patchs/0001-Remove-assert.patch b/config/freertos/crazyflie21/patchs/0001-Remove-assert.patch
new file mode 100644
index 00000000..19d40a0f
--- /dev/null
+++ b/config/freertos/crazyflie21/patchs/0001-Remove-assert.patch
@@ -0,0 +1,25 @@
+From df14a0e01528c304c758bb4f32b1f28cc5fecc19 Mon Sep 17 00:00:00 2001
+From: Antonio cuadros <acuadros1995@gmail.com>
+Date: Mon, 27 Sep 2021 10:05:54 +0200
+Subject: [PATCH] Remove assert
+
+---
+ src/hal/src/radiolink.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/src/hal/src/radiolink.c b/src/hal/src/radiolink.c
+index 738901f3..bfe07865 100644
+--- a/src/hal/src/radiolink.c
++++ b/src/hal/src/radiolink.c
+@@ -161,7 +161,7 @@ void radiolinkSyslinkDispatch(SyslinkPacket *slp)
+   {
+     slp->length--; // Decrease to get CRTP size.
+     // Assert that we are not dopping any packets
+-    ASSERT(xQueueSend(crtpPacketDelivery, &slp->length, 0) == pdPASS);
++    xQueueSend(crtpPacketDelivery, &slp->length, 0);
+     ledseqRun(&seq_linkUp);
+     // If a radio packet is received, one can be sent
+     if (xQueueReceive(txQueue, &txPacket, 0) == pdTRUE)
+-- 
+2.25.1
+