diff --git a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
index a68da6aeb1b33c6350975754c5fa6c6dbea801fd..1a0d00366b9e840ef70bd9c3ab59af196510ee57 100644
--- a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
+++ b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
@@ -46,7 +46,7 @@ struct BusLoadInfo
 };
 class BusLoadEstimation
 {
-    static constexpr uint16_t BUFFER_LEN = 20;
+    static constexpr uint16_t BUFFER_LEN = 50;
 
 public:
     BusLoadEstimation(uint32_t baudRate) : baudRate(baudRate) {}
@@ -90,7 +90,7 @@ private:
     };
 
     CircularBuffer<PacketInfo, BUFFER_LEN> c;
-    mutable FastMutex mutex;
+    FastMutex mutex;
     uint32_t baudRate;
 };
 
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
index 546d5879bed167c150e046c57b134fd1949a7f98..2884c27a409eeaedea8aaaa2a1cfa1a69adb537e 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -22,6 +22,8 @@
 
 #include "CanProtocol.h"
 
+#include <drivers/timer/TimestampTimer.h>
+
 using namespace miosix;
 
 namespace Boardcore
@@ -134,6 +136,8 @@ void CanProtocol::sendMessage(const CanMessage& msg)
                  static_cast<uint32_t>(CanProtocolIdMask::LEFT_TO_SEND));
     packet.length = byteForUint64(msg.payload[0]);
 
+    packet.timestamp = Boardcore::TimestampTimer::getTimestamp();
+
     // Splits payload[0] in the right number of uint8_t
     for (int i = 0; i < packet.length; i++)
         packet.data[i] = msg.payload[0] >> (8 * i);
@@ -262,6 +266,7 @@ void CanProtocol::runReceiver()
 
                     // Add the data to the message
                     msg.payload[msg.length - leftToReceive - 1] = payload;
+                    loadEstimator->addPacket(pkt);
                     nReceived++;
                 }
             }
diff --git a/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
index 1bcb2e3e706076fdf3ca105d27fe27cadd160aaf..56be5fefd231a7f64868d5ae089bf64809674f37 100644
--- a/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
@@ -195,11 +195,10 @@ int main()
 
     CanbusDriver::CanbusConfig cfg{};
     CanbusDriver::AutoBitTiming bt;
-    bt.baudRate      = BAUD_RATE;
-    bt.samplePoint   = SAMPLE_POINT;
-    CanbusDriver* c1 = new CanbusDriver(CAN1, cfg, bt);
-    CanbusDriver* c  = new CanbusDriver(CAN2, cfg, bt);
-    canManager       = new SimpleCanManager(*c, BAUD_RATE, handleCanMessage);
+    bt.baudRate     = BAUD_RATE;
+    bt.samplePoint  = SAMPLE_POINT;
+    CanbusDriver* c = new CanbusDriver(CAN2, cfg, bt);
+    canManager      = new SimpleCanManager(*c, BAUD_RATE, handleCanMessage);
 
     // Allow every message
     Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
@@ -208,7 +207,7 @@ int main()
 
     canManager->start();
     mc.start();
-    const int slp = 5;
+    const int slp = 100;
     for (;;)
     {
         sendNewRequest();
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
index 1d62c6ec7bc112ae773fc5e871145194f7551c7e..37d4b9e86bc6ccafa42a727c71b02d34cd60df4c 100644
--- a/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
@@ -202,10 +202,6 @@ int main()
     bt.baudRate    = BAUD_RATE;
     bt.samplePoint = SAMPLE_POINT;
 
-    CanbusDriver* c1 = new CanbusDriver(CAN1, cfg, bt);
-    if (false && c1)
-    {
-    }
     CanbusDriver* c = new CanbusDriver(CAN2, cfg, bt);
     protocol = new CanProtocol(c, bind(&handleCanMessage, _1), BAUD_RATE);
 
@@ -216,7 +212,7 @@ int main()
     protocol->start();
 
     mc.start();
-    const int slp = 20;
+    const int slp = 10;
     for (;;)
     {
         sendNewRequest();
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp
index 686ebd1840495e3dce8eceb98c6c509ec4fac0fe..e245245af4811f255f0006d5449efbcbf65e4f27 100644
--- a/src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp
@@ -42,21 +42,6 @@ void print(const CanMessage& msg)
 
 int main()
 {
-    GpioPin canA{GPIOB_BASE, 8};
-    GpioPin canB{GPIOB_BASE, 9};
-
-    canA.mode(Mode::ALTERNATE);
-    canB.mode(Mode::ALTERNATE);
-    canA.alternateFunction(9);
-    canB.alternateFunction(9);
-
-    GpioPin can2A{GPIOB_BASE, 12};
-    GpioPin can2B{GPIOB_BASE, 13};
-
-    can2A.mode(Mode::ALTERNATE);
-    can2B.mode(Mode::ALTERNATE);
-    can2A.alternateFunction(9);
-    can2B.alternateFunction(9);
 
     printf("provolone fritto\n");