diff --git a/src/shared/drivers/usart/USART.cpp b/src/shared/drivers/usart/USART.cpp
index 2ba7d4f000960a56867317243144bbe0ea81d5a4..a12a233588542674113d7ff159f19059d0ec2202 100644
--- a/src/shared/drivers/usart/USART.cpp
+++ b/src/shared/drivers/usart/USART.cpp
@@ -570,6 +570,11 @@ STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate)
             initPins(u3tx1::getPin(), 7, u3rx1::getPin(), 7);
             this->serialPortName = std::string("usart3");
             break;
+        case UART4_BASE:
+            this->id = 4;
+            initPins(u4tx1::getPin(), 8, u4rx1::getPin(), 8);
+            this->serialPortName = std::string("uart4");
+            break;
     }
     initialized = false;
     fd          = -1;
@@ -594,8 +599,17 @@ STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate,
             this->id             = 3;
             this->serialPortName = std::string("usart3");
             break;
+        case UART4_BASE:
+            this->id             = 4;
+            this->serialPortName = std::string("uart4");
+            break;
     }
-    initPins(tx, 7, rx, 7);
+
+    if(id<4)
+        initPins(tx, 7, rx, 7);
+    else
+        initPins(tx, 8, rx, 8);
+
     initialized = false;
     fd          = -1;
 }
@@ -610,7 +624,7 @@ STM32SerialWrapper::~STM32SerialWrapper()
 
 bool STM32SerialWrapper::init()
 {
-    if (id > 3)
+    if (id > 4)
     {
         TRACE(
             "[STM32SerialWrapper] USART id greater than 3 is not supported\n");
diff --git a/src/tests/drivers/usart/test-usart.cpp b/src/tests/drivers/usart/test-usart.cpp
index 48bff73590492c88cbabd8909079fb8d471741d4..a4176320e13987c51f4b4af3101a649a6f22cd83 100644
--- a/src/tests/drivers/usart/test-usart.cpp
+++ b/src/tests/drivers/usart/test-usart.cpp
@@ -141,10 +141,17 @@ int main()
     u1rx2::getPin().alternateFunction(7);
     u1tx1::getPin().mode(miosix::Mode::ALTERNATE);
     u1tx1::getPin().alternateFunction(7);
-    u4rx1::getPin().mode(miosix::Mode::ALTERNATE);
-    u4rx1::getPin().alternateFunction(8);
-    u4tx1::getPin().mode(miosix::Mode::ALTERNATE);
-    u4tx1::getPin().alternateFunction(8);
+
+    u2rx1::getPin().mode(miosix::Mode::ALTERNATE);
+    u2rx1::getPin().alternateFunction(7);
+    u2tx1::getPin().mode(miosix::Mode::ALTERNATE);
+    u2tx1::getPin().alternateFunction(7);
+
+
+    // u4rx1::getPin().mode(miosix::Mode::ALTERNATE);
+    // u4rx1::getPin().alternateFunction(8);
+    // u4tx1::getPin().mode(miosix::Mode::ALTERNATE);
+    // u4tx1::getPin().alternateFunction(8);
 
     bool testPassed = true;
     printf("*** SERIAL 3 WORKING!\n");
@@ -158,7 +165,7 @@ int main()
         STM32SerialWrapper usartx(USART1, baudrate);
         usartx.init();
 
-        USART usarty(UART4, baudrate);
+        STM32SerialWrapper usarty(USART2, baudrate);
         // usarty.setOversampling(false);
         // usarty.setStopBits(1);
         // usarty.setWordLength(USART::WordLength::BIT8);