diff --git a/src/shared/drivers/canbus/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol.cpp
index f48d0bba7b53d6fe17452f52ecfa6cfc9557bc6e..2ccf4b63bcd701d442470d501b3ce73a2f34981d 100644
--- a/src/shared/drivers/canbus/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol.cpp
@@ -119,61 +119,72 @@ void CanProtocol::run()
             // Check for maximum size
             if (sourceId < N_BOARDS)
             {
+
                 uint8_t left = 63 - (packet.id & leftToSend);
 
                 // Check if it is the first packet in the sequence
                 if (((packet.id & firstPacket) >> shiftFirstPacket) == 0)
                 {
-                    // if it is we save the id (without the sequence number) the
-                    // number of packet (left to send + 1)
+                    // if it is we save the id (without the sequence number)
+                    // the number of packet (left to send + 1)
                     data[sourceId].length = left + 1;
 
-                    // the number of packet = left to send + 1 since it is the
-                    // first packet
+                    // the number of packet = left to send + 1 since it is
+                    // the first packet
                     data[sourceId].canId = idNoSeq;
 
                     // And we reset nRec
                     data[sourceId].nRec = 0;
                 }
 
-                // if the packet is expected, the length of data - the number of
-                // packet recorded +1 (+1 since we are not counting the last
-                // packet) equals the number of packet left to receive
-                if ((data[sourceId].length - (data[sourceId].nRec + 1)) == left)
+                // we accept the packet only if we already received a first
+                // packet (it is already in data)
+                if (data[sourceId].canId == -1 ||
+                    ((data[sourceId].canId & source) >> shiftSource) ==
+                        sourceId)
                 {
-                    uint64_t tempPayload = 0;
-
-                    // we reassemble the payload
-                    for (int f = 0; f < packet.length; f++)
-                    {
-                        uint64_t tempData = packet.data[f];
-                        tempPayload       = tempPayload | (tempData << (f * 8));
-                    }
-
-                    if (data[sourceId].length - left - 1 >= 0 &&
-                        data[sourceId].length - left - 1 <
-                            32)  // check for index to avoid out of bounds error
+                    // if the packet is expected, the length of data - the
+                    // number of packet recorded +1 (+1 since we are not
+                    // counting the last packet) equals the number of packet
+                    // left to receive
+                    if ((data[sourceId].length - (data[sourceId].nRec + 1)) ==
+                        left)
                     {
-                        // and put it in data
-                        data[sourceId]
-                            .payload[data[sourceId].length - left - 1] =
-                            tempPayload;
-                        data[sourceId].nRec++;
+                        uint64_t tempPayload = 0;
+
+                        // we reassemble the payload
+                        for (int f = 0; f < packet.length; f++)
+                        {
+                            uint64_t tempData = packet.data[f];
+                            tempPayload = tempPayload | (tempData << (f * 8));
+                        }
+
+                        if (data[sourceId].length - left - 1 >= 0 &&
+                            data[sourceId].length - left - 1 <
+                                32)  // check for index to avoid out of bounds
+                                     // error
+                        {
+                            // and put it in data
+                            data[sourceId]
+                                .payload[data[sourceId].length - left - 1] =
+                                tempPayload;
+                            data[sourceId].nRec++;
+                        }
                     }
-                }
-                // If we have received the right number of packet
-                if (data[sourceId].nRec == data[sourceId].length &&
-                    data[sourceId].nRec != 0)
-                {
+                    // If we have received the right number of packet
+                    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
+                            miosix::Lock<miosix::FastMutex> l(mutex);
+                            buffer.put(data[sourceId]);
+                        }
+
+                        // Empties the struct
+                        data[sourceId].canId  = -1;
+                        data[sourceId].length = 0;
                     }
-
-                    // Empties the struct
-                    data[sourceId].canId  = -1;
-                    data[sourceId].length = 0;
                 }
             }
         }
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index 171db2aee1d7c6fa8cf964879d9331817c08e175..529abb69361fdc2faa63afa85e88a7619ce4e2d3 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -130,7 +130,7 @@ int main()
     toSend2.payload[3] = 1;
     std::thread secondSend(sendData, &protocol, &toSend2);
     TRACE("start \n");
-
+    int error = 0;
     for (;;)
     {
         protocol.waitBufferEmpty();
@@ -138,6 +138,7 @@ int main()
         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);
@@ -150,5 +151,9 @@ int main()
         {
             TRACE("OK :) id  %lu\n", temp.canId);
         }
+        if (error != 0)
+        {
+            TRACE("Number of Error  d\n", error);
+        }
     }
 }