diff --git a/src/shared/drivers/canbus/CanDriver/CanDriver.cpp b/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
index 5597540572af5ce8589eb92cf75b3b876d28020c..ea7d92dfa282153d96d9361ae56d43b3d28c8f66 100644
--- a/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
+++ b/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
@@ -96,14 +96,34 @@ CanbusDriver::CanbusDriver(CAN_TypeDef* can, CanbusConfig config,
     // Enable interrupts
     can->IER |= CAN_IER_FMPIE0 | CAN_IER_FMPIE1 | CAN_IER_TMEIE;
 
-    NVIC_EnableIRQ(CAN1_RX0_IRQn);
-    NVIC_SetPriority(CAN1_RX0_IRQn, 14);
+    // Enable the corresponding interrupts
+    if (can == CAN1)
+    {
+        NVIC_EnableIRQ(CAN1_RX0_IRQn);
+        NVIC_SetPriority(CAN1_RX0_IRQn, 14);
 
-    NVIC_EnableIRQ(CAN1_RX1_IRQn);
-    NVIC_SetPriority(CAN1_RX1_IRQn, 14);
+        NVIC_EnableIRQ(CAN1_RX1_IRQn);
+        NVIC_SetPriority(CAN1_RX1_IRQn, 14);
 
-    NVIC_EnableIRQ(CAN1_TX_IRQn);
-    NVIC_SetPriority(CAN1_TX_IRQn, 14);
+        NVIC_EnableIRQ(CAN1_TX_IRQn);
+        NVIC_SetPriority(CAN1_TX_IRQn, 14);
+    }
+    else if (can == CAN2)
+    {
+        NVIC_EnableIRQ(CAN2_RX0_IRQn);
+        NVIC_SetPriority(CAN2_RX0_IRQn, 14);
+
+        NVIC_EnableIRQ(CAN2_RX1_IRQn);
+        NVIC_SetPriority(CAN2_RX1_IRQn, 14);
+
+        NVIC_EnableIRQ(CAN2_TX_IRQn);
+        NVIC_SetPriority(CAN2_TX_IRQn, 14);
+    }
+    else
+    {
+        PrintLogger ls = l.getChild("constructor");
+        LOG_ERR(ls, "Not supported peripheral");
+    }
 }
 
 CanbusDriver::~CanbusDriver() { ClockUtils::disablePeripheralClock(can); }
@@ -202,6 +222,13 @@ void CanbusDriver::init()
 bool CanbusDriver::addFilter(FilterBank filter)
 {
     PrintLogger ls = l.getChild("addfilter");
+    uint8_t index  = filterIndex;
+
+    // CAN2 filters start from the 15th position
+    if (can == CAN2)
+    {
+        index = index + 14;
+    }
 
     if (isInit)
     {
@@ -209,21 +236,23 @@ bool CanbusDriver::addFilter(FilterBank filter)
         return false;
     }
 
-    if (filterIndex == NUM_FILTER_BANKS)
+    if (index >= NUM_FILTER_BANKS)
     {
         LOG_ERR(ls, "Cannot add filter: no more filter banks available");
         return false;
     }
 
-    can->sFilterRegister[filterIndex].FR1 = filter.FR1;
-    can->sFilterRegister[filterIndex].FR2 = filter.FR2;
+    // NOTE: the filters are set in CAN1 peripheral because the filter registers
+    // between the peripherals are in common.
+    CAN1->sFilterRegister[index].FR1 = filter.FR1;
+    CAN1->sFilterRegister[index].FR2 = filter.FR2;
 
-    can->FM1R |= (filter.mode == FilterMode::MASK ? 0 : 1) << filterIndex;
-    can->FS1R |= (filter.scale == FilterScale::DUAL16 ? 0 : 1) << filterIndex;
-    can->FFA1R |= (filter.fifo & 0x1) << filterIndex;
+    CAN1->FM1R |= (filter.mode == FilterMode::MASK ? 0 : 1) << index;
+    CAN1->FS1R |= (filter.scale == FilterScale::DUAL16 ? 0 : 1) << index;
+    CAN1->FFA1R |= (filter.fifo & 0x1) << index;
 
     // Enable the filter
-    can->FA1R |= 1 << filterIndex;
+    CAN1->FA1R |= 1 << index;
 
     ++filterIndex;
 
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
index bc1bc8b1c29e69fef80e53a7038a222cd0f2cb10..57afa9bf9e27e2d09463a89cfe610d62c1bcf7e0 100644
--- a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
@@ -42,21 +42,43 @@ void print(const CanMessage& msg)
 
 int main()
 {
-    // Prepare the cab driver
+    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");
+
+    // // Prepare the cab driver
     CanbusDriver::CanbusConfig config;
     config.loopback = true;
     CanbusDriver::AutoBitTiming bitTiming;
-    bitTiming.baudRate    = 500 * 1000;
+    bitTiming.baudRate    = 50 * 1000;
     bitTiming.samplePoint = 87.5f / 100.0f;
-    CanbusDriver* driver  = new CanbusDriver(CAN1, config, bitTiming);
 
-    // Prepare the can driver
-    CanProtocol protocol(driver, print);
+    // To make the CAN2 work the driver must be created also for CAN1
+    // TODO change this thing
+    CanbusDriver* driver1 = new CanbusDriver(CAN1, config, bitTiming);
+    CanbusDriver* driver2 = new CanbusDriver(CAN2, config, bitTiming);
+
+    // // Prepare the can driver
+    CanProtocol protocol(driver2, print);
 
     // Add a filter to allow every message
     Mask32FilterBank f2(0, 0, 1, 1, 0, 0, 0);
-    driver->addFilter(f2);
-    driver->init();
+    driver2->addFilter(f2);
+    driver2->init();
 
     // Start the protocol
     protocol.start();