diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4df2e09419a93d1a797f9f094a98c6c3e0c0301a..c7c74ae2dea01fdf9b725f65b7dcb8e1f3feb510 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,7 +80,7 @@ add_executable(test-sensormanager src/tests/test-sensormanager.cpp)
 sbs_target(test-sensormanager stm32f429zi_skyward_death_stack_x)
 
 add_executable(test-serial src/tests/test-serial.cpp)
-sbs_target(test-serial stm32f407vg_stm32f4discovery)
+sbs_target(test-serial stm32f429zi_stm32f4discovery)
 
 add_executable(test-taskscheduler src/tests/scheduler/test-taskscheduler.cpp)
 sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery)
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index cf78a0c341fd185d846b8fc077c48e8d1497552d..c692b7d051ce2bc8ecb28bd087e8ac3d39adf30b 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -130,7 +130,6 @@ public:
         tempLen--;
 
         can->send(packet);
-        TRACE("tosend len %d\n", toSend.len);
 
         for (int i = 1; i < toSend.len; i++)
         {
@@ -142,7 +141,7 @@ public:
             {
                 packet.data[k] = toSend.payload[i] << (8 * k);
             }
-            TRACE("packetlen %d\n, dato %d\n", packet.length, packet.data[0]);
+
             can->send(packet);
             tempLen--;
         }
@@ -182,8 +181,7 @@ protected:
                         data[sourceId].canId =
                             packet.id >> 7;  // discard the sequence number
                     }
-                    TRACE("pakcet %d, nrec %d, left %lu\n", packet.data[0],
-                          data[sourceId].nRec, (packet.id & idMask.leftToSend));
+
                     if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
                         (packet.id & idMask.leftToSend))
                     {
@@ -202,7 +200,8 @@ protected:
                         data[sourceId].nRec++;
                     }
 
-                    if (data[sourceId].nRec == data[sourceId].len)
+                    if (data[sourceId].nRec == data[sourceId].len &&
+                        data[sourceId].nRec != 0)
                     {
                         mutex.lock();
                         buffer.put(data[sourceId]);
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index e2126819497d35abdf939256d70a0e4bd8fb3caf..52627848383604022bfb6929ab8317a826a79d18 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -23,7 +23,6 @@
  */
 
 #include <drivers/canbus/CanProtocol.h>
-#include <inttypes.h>
 
 #include <thread>
 
@@ -49,21 +48,33 @@ using CanRX = Gpio<GPIOA_BASE, 11>;
 using CanTX = Gpio<GPIOA_BASE, 12>;
 #endif
 
-#define SLP 5000
+#define SLP 500
 
-void sendData(CanProtocol* protocol, CanData toSend)
+void sendData(CanProtocol* protocol, CanData* toSend)
 {
     while (true)
     {
         TRACE("send\n");
-        (*protocol).sendCan(toSend);
+        (*protocol).sendCan(*toSend);
         Thread::sleep(SLP);
     }
+}  // todo mchange the source id and send packet at the same time
+bool equal(CanData* first, CanData* second)
+{
+    if ((*first).canId != (*second).canId || (*first).len != (*second).len)
+    {
+        return false;
+    }
+    for (int i = 0; i < (*first).len; i++)
+    {
+        if ((*first).payload[i] != (*second).payload[i])
+            return false;
+    }
+    return true;
 }
 
 int main()
 {
-
     {
         miosix::FastInterruptDisableLock dLock;
 
@@ -78,50 +89,54 @@ int main()
         CanTX::alternateFunction(9);
 #endif
     }
-
+    TRACE("start \n");
     CanbusDriver::CanbusConfig cfg{};
     CanbusDriver::AutoBitTiming bt;
     bt.baudRate    = BAUD_RATE;
     bt.samplePoint = SAMPLE_POINT;
-
+    TRACE("start \n");
     CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
+    TRACE("start \n");
     CanProtocol protocol(c);
     // Allow every message
+    TRACE("start \n");
     Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
-
+    TRACE("start \n");
     c->addFilter(f2);
     c->init();
     protocol.start();
-    CanData toSend;
-    toSend.canId      = 0x01;
-    toSend.len        = 3;
-    toSend.payload[0] = 1;
-    toSend.payload[1] = 2;
-    toSend.payload[2] = 3;
-    std::thread second(sendData, &protocol, toSend);
+    CanData toSend1;
+    toSend1.canId      = 0x01;
+    toSend1.len        = 3;
+    toSend1.payload[0] = 1;  // 0xffffffff;
+    toSend1.payload[1] = 2;
+    toSend1.payload[2] = 3;  // 78022;
+    std::thread firstSend(sendData, &protocol, &toSend1);
+    CanData toSend2;
+    toSend2.canId      = 0x100;
+    toSend2.len        = 4;
+    toSend2.payload[0] = 0xffffff;
+    toSend2.payload[1] = 2;
+    toSend2.payload[2] = 0x123ff;
+    toSend2.payload[3] = 1;
+    TRACE("start \n");
+    std::thread secondSend(sendData, &protocol, &toSend2);
+    TRACE("start \n");
     for (;;)
     {
-        TRACE("start \n");
+
         protocol.waitEmpty();
         CanData temp = protocol.getPacket();
-        if (temp.canId != toSend.canId || temp.len != toSend.len ||
-            temp.payload[0] != toSend.payload[0] ||
-            temp.payload[1] != toSend.payload[1] ||
-            temp.payload[2] != toSend.payload[2])
+        TRACE("received packet \n");
+        if (!equal(&temp, &toSend1))
         {
             TRACE("Error\n");
-            TRACE("Expected id %lu, received  %lu\n", toSend.canId, temp.canId);
-            TRACE("Expected len %d , received %d\n", toSend.len, temp.len);
-            TRACE(
-                "Expected payload 0  %llu , received  "
-                "%llu\n",
-                toSend.payload[0], temp.payload[0]);
-            TRACE("Expected payload 1 %llu, received  %llu\n",
-                  toSend.payload[1], temp.payload[1]);
-            TRACE(
-                "Expected payload 2  %llu, received  "
-                "%llu\n",
-                toSend.payload[2], temp.payload[2]);
+            TRACE("Received  %lu\n", temp.canId);
+            TRACE("Received %d\n", temp.len);
+            for (int i = 0; i < temp.len; i++)
+            {
+                TRACE("Received payload %d:  %llu\n", i, temp.payload[i]);
+            }
         }
         else
         {