diff --git a/src/shared/drivers/canbus/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol.cpp
index 1f405fabd1ba828a5656c8273d5dd8afee58f4e4..f48d0bba7b53d6fe17452f52ecfa6cfc9557bc6e 100644
--- a/src/shared/drivers/canbus/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol.cpp
@@ -27,8 +27,8 @@ namespace Boardcore
 
 namespace Canbus
 {
-
-CanData data[N_PACKET];
+// For each board contains the packet that we are re-assembling
+CanData data[N_BOARDS];
 
 CanProtocol::CanProtocol(CanbusDriver* can) { this->can = can; }
 
@@ -60,9 +60,10 @@ void CanProtocol::sendData(CanData dataToSend)
 
     // Send the first packet
     packet.ext = true;
-    packet.id  = (tempId << shiftSequentialInfo) | firstPacket |
-                (63 - (tempLen & leftToSend));
+    // create the id for the first packet
+    packet.id = (tempId << shiftSequentialInfo) | (63 - (tempLen & leftToSend));
     packet.length = byteForInt(dataToSend.payload[0]);
+    // Splits payload[0] in the right number of uint8_t
     for (int k = 0; k < packet.length; k++)
         packet.data[k] = dataToSend.payload[0] >> (8 * k);
     can->send(packet);
@@ -71,10 +72,11 @@ void CanProtocol::sendData(CanData dataToSend)
     for (int i = 1; i < dataToSend.length; i++)
     {
         tempId = dataToSend.canId;
-        packet.id =
-            (tempId << shiftSequentialInfo) | (63 - (tempLen & leftToSend));
+        // create the id for the remaining packets
+        packet.id = (tempId << shiftSequentialInfo) | firstPacket |
+                    (63 - (tempLen & leftToSend));
         packet.length = byteForInt(dataToSend.payload[i]);
-
+        // Splits payload[i] in the right number of uint8_t
         for (int k = 0; k < packet.length; k++)
             packet.data[k] = dataToSend.payload[i] >> (8 * k);
 
@@ -111,57 +113,67 @@ void CanProtocol::run()
 
             // Discard the sequence number
             uint32_t idNoSeq = packet.id >> shiftSequentialInfo;
+            // Extract the sourceID
             uint8_t sourceId = (idNoSeq & source) >> shiftSource;
 
             // Check for maximum size
-            if (sourceId < N_PACKET)
+            if (sourceId < N_BOARDS)
             {
-                if (data[sourceId].canId == -1 ||
-                    ((data[sourceId].canId & source) >> shiftSource) ==
-                        sourceId)
+                uint8_t left = 63 - (packet.id & leftToSend);
+
+                // Check if it is the first packet in the sequence
+                if (((packet.id & firstPacket) >> shiftFirstPacket) == 0)
                 {
-                    uint8_t left = 63 - (packet.id & leftToSend);
+                    // 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
+                    data[sourceId].canId = idNoSeq;
 
-                    // Check if it is the first packet in the sequence
-                    if ((packet.id & firstPacket) >> shiftFirstPacket)
+                    // 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)
+                {
+                    uint64_t tempPayload = 0;
+
+                    // we reassemble the payload
+                    for (int f = 0; f < packet.length; f++)
                     {
-                        data[sourceId].length = left + 1;
-                        data[sourceId].canId  = idNoSeq;
+                        uint64_t tempData = packet.data[f];
+                        tempPayload       = tempPayload | (tempData << (f * 8));
                     }
 
-                    if ((data[sourceId].length - (data[sourceId].nRec + 1)) ==
-                        left)
+                    if (data[sourceId].length - left - 1 >= 0 &&
+                        data[sourceId].length - left - 1 <
+                            32)  // check for index to avoid out of bounds error
                     {
-                        uint64_t tempPayload = 0;
-
-                        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
-                        {
-                            data[sourceId]
-                                .payload[data[sourceId].length - left - 1] =
-                                tempPayload;
-                            data[sourceId].nRec++;
-                        }
+                        // and put it in data
+                        data[sourceId]
+                            .payload[data[sourceId].length - left - 1] =
+                            tempPayload;
+                        data[sourceId].nRec++;
                     }
-
-                    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]);
-
-                        // Empties the struct
-                        data[sourceId].canId  = -1;
-                        data[sourceId].nRec   = 0;
-                        data[sourceId].length = 0;
                     }
+
+                    // Empties the struct
+                    data[sourceId].canId  = -1;
+                    data[sourceId].length = 0;
                 }
             }
         }
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index 7e9efa91f4269209c9ec6649f164ae8f6f6ba68a..c68c859dc127247d23f0ee155163b3c6c764cd15 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -28,7 +28,7 @@
 
 #include "Canbus.h"
 
-#define N_PACKET 3  ///< Number of boards on the bus.
+#define N_BOARDS 3  ///< Number of boards on the bus.
 
 namespace Boardcore
 {
@@ -58,29 +58,36 @@ namespace Canbus
  * @brief Enumeration that contains masks of the elements composing the can
  * packet id without sequential information.
  */
-enum IDMask
+enum IDMask : uint32_t
 {
-    priority         = 0x3C0000,
-    shiftPriority    = 18,
-    type             = 0x03F000,
-    shiftType        = 12,
-    source           = 0x000F00,
-    shiftSource      = 8,
-    destination      = 0x0000F0,
-    shiftDestination = 4,
-    idType           = 0x00000F,
-    shiftIdType      = 0
+    priority = 0x3C0000,
+
+    type        = 0x03F000,
+    source      = 0x000F00,
+    destination = 0x0000F0,
+    idType      = 0x00000F
 };
 
 /**
  * @brief @brief Enumeration that contains masks of the elements composing the
  * sequential information.
  */
-enum SequentialInformation
+enum SequentialInformation : uint8_t
+{
+    firstPacket = 0x40,
+    leftToSend  = 0x3F
+};
+
+enum ShiftInformation : uint8_t
 {
-    firstPacket         = 0x40,
+    // Shift info of IDMask
+    shiftPriority    = 18,
+    shiftType        = 12,
+    shiftSource      = 8,
+    shiftDestination = 4,
+    shiftIdType      = 0,
+    // Shift info of SequentialInformation
     shiftFirstPacket    = 6,
-    leftToSend          = 0x3F,
     shiftLeftToSend     = 0,
     shiftSequentialInfo = 7
 };
@@ -109,11 +116,13 @@ struct CanData
 class CanProtocol : public ActiveObject
 {
 private:
-    // TODO: Add mutex and create get data in can protocol
+    // the physical implementation of the CanBus
+    CanbusDriver* can;
+
+    // the buffer used to store the completed CanData
+    IRQCircularBuffer<CanData, N_BOARDS> buffer;
+
     miosix::FastMutex mutex;
-    CanbusDriver* can;  // the physical can
-    IRQCircularBuffer<CanData, N_PACKET>
-        buffer;  // the buffer used to send data from CanProtocol to CanHandler
 
 public:
     /**
@@ -144,7 +153,7 @@ public:
      * correct sequential id.
      * @warning requires @param data to be not empty.
      *
-     * @param data Contains the id e the data of the packet to send.
+     * @param data Contains the id and the data of the packet to send.
      */
     void sendData(CanData dataToSend);
 
@@ -158,8 +167,8 @@ private:
      * @brief Keeps listening on the canbus for packets.
      *
      * Once a packet is received, it checks if it is expected (that id is
-     * already present in data), if that is the case, it is added to the list.
-     * Once we receive the correct amount of packet we send it to can handler.
+     * already present in data), if it is the case, it's added to the list.
+     * Once we receive the correct amount of packet we offer it in buffer.
      *
      * For now if a packet is missed/received in the wrong order the whole
      * packet will be lost once we receive a new first packet without warning
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index b370187437d0357c916bb6223e4e4ebeff0e03b4..171db2aee1d7c6fa8cf964879d9331817c08e175 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -57,7 +57,7 @@ void sendData(CanProtocol* protocol, CanData* toSend)
     while (true)
     {
 
-        TRACE("send\n");
+        TRACE("send id %d\n", toSend->canId);
         {
             miosix::Lock<miosix::FastMutex> l(mutex);
             (*protocol).sendData(*toSend);