diff --git a/CMakeLists.txt b/CMakeLists.txt
index c7c74ae2dea01fdf9b725f65b7dcb8e1f3feb510..e69d0759e5b07c981fc6fb1bbfdc0996171d3cc7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -160,13 +160,13 @@ sbs_target(test-ada stm32f429zi_skyward_death_stack_v3)
 #                               Tests - Drivers                               #
 #-----------------------------------------------------------------------------#
 add_executable(test-can-protocol src/tests/drivers/canbus/test-can-protocol.cpp)
-sbs_target(test-can-protocol stm32f429zi_stm32f4discovery)
+sbs_target(test-can-protocol stm32f429zi_skyward_pyxis_auxiliary)
 
 add_executable(test-canbus-loopback src/tests/drivers/canbus/test-canbus-loopback.cpp)
 sbs_target(test-canbus-loopback stm32f429zi_stm32f4discovery)
 
 add_executable(test-canbus-2way src/tests/drivers/canbus/test-canbus-2way.cpp)
-sbs_target(test-canbus-2way stm32f429zi_stm32f4discovery)
+sbs_target(test-canbus-2way stm32f429zi_skyward_pyxis_auxiliary)
 
 add_executable(test-dma-spi src/tests/drivers/dma/test-dma-spi.cpp)
 sbs_target(test-dma-spi stm32f429zi_stm32f4discovery)
diff --git a/src/shared/drivers/canbus/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol.cpp
index 2ccf4b63bcd701d442470d501b3ce73a2f34981d..5f4fb440179ef5de65c8eaca061790985aa9d410 100644
--- a/src/shared/drivers/canbus/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol.cpp
@@ -36,21 +36,15 @@ CanProtocol::~CanProtocol() { (*can).~CanbusDriver(); }
 
 CanData CanProtocol::getPacket()
 {
-    miosix::Lock<miosix::FastMutex> l(mutex);
-
     if (!buffer.isEmpty())
         return buffer.pop();
     else
         return {};
 }
 
-bool CanProtocol::isBufferEmpty()
-{
-    miosix::Lock<miosix::FastMutex> l(mutex);
-    return buffer.isEmpty();
-}
+bool CanProtocol::isBufferEmpty() { return buffer.isEmpty(); }
 
-void CanProtocol::waitBufferEmpty() { buffer.waitUntilNotEmpty(); }
+void CanProtocol::waitBufferNotEmpty() { buffer.waitUntilNotEmpty(); }
 
 void CanProtocol::sendData(CanData dataToSend)
 {
@@ -71,7 +65,6 @@ void CanProtocol::sendData(CanData dataToSend)
 
     for (int i = 1; i < dataToSend.length; i++)
     {
-        tempId = dataToSend.canId;
         // create the id for the remaining packets
         packet.id = (tempId << shiftSequentialInfo) | firstPacket |
                     (63 - (tempLen & leftToSend));
@@ -175,11 +168,8 @@ void CanProtocol::run()
                     if (data[sourceId].nRec == data[sourceId].length &&
                         data[sourceId].nRec != 0)
                     {
-                        {
-                            // We put the element of data in buffer
-                            miosix::Lock<miosix::FastMutex> l(mutex);
-                            buffer.put(data[sourceId]);
-                        }
+                        // We put the element of data in buffer
+                        buffer.put(data[sourceId]);
 
                         // Empties the struct
                         data[sourceId].canId  = -1;
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index a1a6ed81bac036c3312d8e1f1d2e16d68089b17d..598adf32b4e4c9a459fc4c22d54e38235a2d9e06 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -101,7 +101,7 @@ int main()
     CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
     CanProtocol protocol(c);
     // Allow every message
-    Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
+    Mask32FilterBank f2(0, 0, 1, 1, 0, 0, 0);
 
     c->addFilter(f2);
     c->init();
@@ -131,15 +131,15 @@ int main()
     int error = 0;
     for (int f = 0; f < 100000; f++)
     {
-        protocol.waitBufferEmpty();
+        protocol.waitBufferNotEmpty();
         CanData temp = protocol.getPacket();
         // TRACE("received packet \n");
         if ((!equal(&temp, &toSend1) && !equal(&temp, &toSend2)))
         {
             error++;
             TRACE("Error\n");
-            TRACE("Received  %lu\n", temp.canId);
-            TRACE("Received %d\n", temp.length);
+            TRACE("Received id %lu\n", temp.canId);
+            TRACE("length %d\n", temp.length);
             for (int i = 0; i < temp.length; i++)
             {
                 TRACE("Received payload %d:  %llu,\n", i, temp.payload[i]);
@@ -154,4 +154,5 @@ int main()
     {
         TRACE("Number of Error  %d\n", error);
     }
+    free(c);
 }