diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5352d725ddc0f3e19aa9932c03a262cc4933a085..29e422e7ee396b497ea084cf3e90662abd9976e8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -200,8 +200,11 @@ sbs_target(test-can-filters stm32f429zi_skyward_pyxis_auxiliary)
 add_executable(test-can-loopback src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp)
 sbs_target(test-can-loopback stm32f429zi_skyward_death_stack_x)
 
-add_executable(test-can-protocol src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp)
-sbs_target(test-can-protocol stm32f429zi_skyward_death_stack_x)
+add_executable(test-can-protocol-loopback src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp)
+sbs_target(test-can-protocol-loopback stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-can-protocol-2way src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp)
+sbs_target(test-can-protocol-2way stm32f429zi_skyward_pyxis_auxiliary)
 
 add_executable(test-dsgamma src/tests/drivers/test-dsgamma.cpp)
 sbs_target(test-dsgamma stm32f429zi_stm32f4discovery)
diff --git a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
index 9b76306ebda9b1a0de93ec1fcd42f16266c8dab7..a68da6aeb1b33c6350975754c5fa6c6dbea801fd 100644
--- a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
+++ b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
@@ -38,18 +38,17 @@ namespace Boardcore
 namespace Canbus
 {
 
+struct BusLoadInfo
+{
+    float payloadBitRate;
+    float totalBitRate;
+    float loadPercent;
+};
 class BusLoadEstimation
 {
     static constexpr uint16_t BUFFER_LEN = 20;
 
 public:
-    struct BusLoadInfo
-    {
-        float payloadBitRate;
-        float totalBitRate;
-        float loadPercent;
-    };
-
     BusLoadEstimation(uint32_t baudRate) : baudRate(baudRate) {}
 
     void addPacket(CanPacket p)
@@ -75,8 +74,9 @@ public:
         for (size_t i = 0; i < c.count(); ++i)
         {
             sizePayload += c.get(i).dataLength * 8;
+            sizeFrames += ((54 + c.get(i).dataLength * 8 - 1) / 4);
         }
-        sizeFrames = sizePayload + (64 + 8) * c.count();
+        sizeFrames += sizePayload + 64 * c.count();
 
         return BusLoadInfo{(sizePayload / dt), (sizeFrames / dt),
                            ((sizeFrames / dt) / baudRate) * 100};
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
index 69f660aa3936989c89e05754d75ca93284a8c44c..473a1f2c469fd08f61f3e273cc5e9667ca945b36 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -206,6 +206,10 @@ bool CanProtocol::enqueueSimplePacket(uint8_t priority, uint8_t primaryType,
     return enqueueMsg(msg);
 }
 
+BusLoadInfo CanProtocol::getLoad(){
+    return loadEstimator->getLoadInfo();
+}
+
 void CanProtocol::runReceiver()
 {
     CanMessage msg;
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
index ac1998497823b98bc78791df099590c0f4f5f1e0..1c2942a45f742c9ebf623007a402ce80225d6f02 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
@@ -137,12 +137,17 @@ public:
     bool enqueueData(uint8_t priority, uint8_t primaryType, uint8_t source,
                      uint8_t destination, uint8_t secondaryType, const T& t);
 
+    /**
+     * @brief Return can load.
+     */
+    BusLoadInfo getLoad();
+
 private:
     /**
      * @brief Blocking send function, puts the CanMessage object on the bus.
      *
-     * Takes a CanMessage object, splits it into multiple CanPackets with the
-     * correct sequential id.
+     * Takes a CanMessage object, splits it into multiple CanPackets with
+     * the correct sequential id.
      *
      * @param msg Contains the id and the data of the packet to send.
      */
diff --git a/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
index 3fd1440dd945e7d2fb69dda142fe5972884fcad6..7d75b8b7eba78b4e7e12b2de78dd14cc33fd21bb 100644
--- a/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
@@ -174,9 +174,8 @@ public:
                     totalPackets, missedDeadline, lostPackets, res.mean,
                     res.maxValue, res.minValue, bufferFull);
 
-                Canbus::BusLoadEstimation::BusLoadInfo info
-                    __attribute__((unused)) =
-                        canManager->getLoadSensor().getLoadInfo();
+                Canbus::BusLoadInfo info __attribute__((unused)) =
+                    canManager->getLoadSensor().getLoadInfo();
                 printf(
                     "Payload rate: %.2f kbps, Frame rate: %.2f kbps, Load: "
                     "%.2f %%\n",
diff --git a/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp b/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
index 06559020e6eaee2e933d2ce66696ec2a1c5baf44..19d4f942bb735c4a5e917a1e2b327b3efe96800d 100644
--- a/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
@@ -64,8 +64,8 @@ public:
     {
         while (!shouldStop())
         {
-            long long start                     = miosix::getTick();
-            BusLoadEstimation::BusLoadInfo info = ble.getLoadInfo();
+            long long start  = miosix::getTick();
+            BusLoadInfo info = ble.getLoadInfo();
             LOG_INFO(l,
                      "payload rate: {:.2f} kbps, total rate: {:.2f} kbps, "
                      "utilization: {:.2f}%",
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53ff65c4a473a5345104a0bca9961e91efcadd35
--- /dev/null
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
@@ -0,0 +1,222 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Luca Erbetta
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+// Sends packets on the canbus and waits for a response, measuring the response
+// time. If a response takes more than MSG_LOST_DEADLINE, the packet is
+// considered lost. It may also receive requests from other canbus devices, to
+// which it will respond
+
+#include <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+#include <miosix.h>
+#include <utils/Debug.h>
+#include <utils/Stats/Stats.h>
+#include <utils/collections/CircularBuffer.h>
+
+#include <functional>
+#include <string>
+
+constexpr uint32_t BAUD_RATE         = 500 * 1000;
+constexpr float SAMPLE_POINT         = 87.5f / 100.0f;
+constexpr uint32_t MSG_DEADLINE      = 100;  // ms
+constexpr uint32_t MSG_LOST_DEADLINE = 400;  // ms
+
+using namespace std;
+using namespace placeholders;
+using namespace Boardcore;
+using namespace Canbus;
+using namespace miosix;
+
+CanProtocol* protocol;
+
+struct CanMsg
+{
+    uint32_t id;
+    uint32_t ts;
+};
+
+CircularBuffer<CanMsg, 64> msgs;
+FastMutex mutexMsgs;
+
+void handleCanMessage(CanMessage packet)
+{
+
+    if (packet.getPriority() == 0x00)  // This is a request
+    {
+        PressureData temp = pressureDataFromCanMessage(packet);
+        // we resend the same packet
+        protocol->enqueueData(0x01, packet.getPrimaryType(), packet.getSource(),
+                              packet.getDestination(),
+                              packet.getSecondaryType(), temp);
+    }
+    else if (packet.getPriority() == 0x1)  // This is a response to a request
+    {
+        Lock<FastMutex> l(mutexMsgs);
+
+        for (size_t i = 0; i < msgs.count(); ++i)
+        {
+            CanMsg& msg = msgs.get(i);
+            if (msg.id == packet.getPrimaryType())
+            {
+                msg.id = 0;
+                msg.ts = getTick() - msg.ts;
+                break;
+            }
+        }
+    }
+}
+
+uint8_t seq = 1;
+void sendNewRequest()
+{
+    CanMessage message;
+    uint8_t id;
+    if (seq >= 0x3F)
+    {
+        seq = 1;
+    }
+    id = seq;
+    seq++;
+
+    if (id % 2 == 0)
+    {
+        id = 0x3F - id;
+    }
+
+    {
+        Lock<FastMutex> l(mutexMsgs);
+        msgs.put({id, (uint32_t)getTick()});
+    }
+    PressureData f;
+    f.pressureTimestamp = 0x12345678;
+    f.pressure          = 9876;
+    protocol->enqueueData(0x00, id, 0x01, 0x00, 0x00, f);
+}
+
+class MessageCollector : public ActiveObject
+{
+public:
+    void run() override
+    {
+        unsigned int c = 0;
+        while (!shouldStop())
+        {
+            uint32_t tick = (uint32_t)getTick();
+            {
+                Lock<FastMutex> l(mutexMsgs);
+                if (msgs.isFull())
+                {
+                    ++bufferFull;
+                }
+                while (!msgs.isEmpty())
+                {
+                    CanMsg msg = msgs.get();
+                    if (msg.id == 0 || tick - msg.ts > MSG_LOST_DEADLINE)
+                    {
+                        msgs.pop();
+
+                        ++totalPackets;
+                        if (msg.id == 0)
+                        {
+                            msgstats.add(msg.ts * 1.0f);
+                            if (msg.ts > MSG_DEADLINE)
+                            {
+                                ++missedDeadline;
+                            }
+                        }
+                        else
+                        {
+                            // No response
+                            ++lostPackets;
+                        }
+                    }
+                    else
+                    {
+                        break;
+                    }
+                }
+            }
+            if (c % 10 == 0)
+            {
+                StatsResult res __attribute__((unused)) = msgstats.getStats();
+
+                printf(
+                    "Total packets: %u, Missed deadlines: %u, Lost packets: "
+                    "%u, Mean ping: %.2f, "
+                    "Max ping: %.0f, Min ping: %.0f, Buffer full: %u\n",
+                    totalPackets, missedDeadline, lostPackets, res.mean,
+                    res.maxValue, res.minValue, bufferFull);
+
+                Canbus::BusLoadInfo info __attribute__((unused)) =
+                    protocol->getLoad();
+                printf(
+                    "Payload rate: %.2f kbps, Frame rate: %.2f kbps, Load: "
+                    "%.2f %%\n",
+                    info.payloadBitRate / 1000.0f, info.totalBitRate / 1000.0f,
+                    info.loadPercent);
+            }
+            ++c;
+            Thread::sleepUntil(tick + MSG_DEADLINE);
+        }
+    }
+
+private:
+    unsigned int totalPackets   = 0;
+    unsigned int lostPackets    = 0;
+    unsigned int missedDeadline = 0;
+    unsigned int bufferFull     = 0;
+    Stats msgstats;
+};
+
+MessageCollector mc;
+
+int main()
+{
+    Logging::startAsyncLogger();
+
+    CanbusDriver::CanbusConfig cfg{};
+    cfg.nart = 0;
+    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);
+    protocol = new CanProtocol(c, bind(&handleCanMessage, _1), BAUD_RATE);
+
+    // Allow every message _1
+    Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
+    c->addFilter(f2);
+    c->init();
+    protocol->start();
+
+    mc.start();
+    const int slp = 20;
+    for (;;)
+    {
+        sendNewRequest();
+        Thread::sleep(slp);
+    }
+}
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp
similarity index 100%
rename from src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
rename to src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp