From 6370560ddf55542a03e01a46b8d3e0934b364d53 Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Sat, 10 Jun 2023 13:30:27 +0000
Subject: [PATCH] [USART] Improved USART driver

- Checked and fixed the baudrate calculation and used `getAPBPeripheralsClock`;
- Removed init method: Some checks are present only as asserts, but the most part of the initialization is performed in the constructor. This usually won't be an issue till we don't try to create a non supported USART (an assert is used to check for this). Removing the init method we delete the issue on forgetting to initialize the bus;
- Changed interface for read: USART and STM32SerialWrapper have `readBlocking` (waits for at least one byte to be received) while only USART has the non-blocking `read` method that returns true if the driver received something and false otherwise; Also, both methods are overloaded to support or not the return of the number of bytes read;
- Removed pins from the `USART.h` file;
- Avoided internally initializing pins in the STM32SerialWrapper driver. We will always have to initialize them externally;
- Enhanced the ifdefs present in the driver to address peripherals and not targets;
- Now baudrate is passed as an integer, so the driver is not limited to the standard baudrates. Anyway the list of default baudrate values is present in the documentation;
- Removed all the other wrappers of the miosix serial driver so that all the codebase uses the USART driver;
- Enhanced documentation;
- Small code refactoring
- Enhanced ISR code
---
 CMakeLists.txt                                |  10 +-
 src/entrypoints/runcam-settings.cpp           |   5 +-
 src/shared/drivers/runcam/Runcam.cpp          |  39 +-
 src/shared/drivers/runcam/Runcam.h            |  17 +-
 src/shared/drivers/runcam/RuncamSerial.h      | 169 ------
 src/shared/drivers/usart/USART.cpp            | 497 ++++++++----------
 src/shared/drivers/usart/USART.h              | 295 ++++++-----
 .../SerialTransceiver/SerialTransceiver.h     |   2 +-
 src/shared/sensors/MBLoadCell/MBLoadCell.cpp  |  28 +-
 src/shared/sensors/MBLoadCell/MBLoadCell.h    |  11 +-
 src/shared/sensors/UBXGPS/UBXGPSSerial.cpp    |  22 +-
 src/shared/sensors/UBXGPS/UBXGPSSerial.h      |  11 +-
 src/shared/sensors/VN100/VN100.cpp            |  37 +-
 src/shared/sensors/VN100/VN100.h              |   9 +-
 src/shared/sensors/VN100/VN100Serial.h        | 185 -------
 src/shared/utils/SerialInterface.h            | 184 -------
 .../algorithms/NAS/test-nas-parafoil.cpp      |   3 +-
 src/tests/drivers/test-MBLoadCell.cpp         |   6 +-
 src/tests/drivers/test-mavlink.cpp            |   2 +-
 src/tests/drivers/test-vn100serial.cpp        |  71 ---
 src/tests/drivers/usart/test-usart.cpp        | 197 ++++---
 src/tests/radio/test-mavlinkdriver.cpp        |  13 +-
 src/tests/sensors/test-ubxgps-serial.cpp      |   3 +-
 src/tests/sensors/test-vn100.cpp              |   3 +-
 src/tests/test-max485.cpp                     |  55 +-
 25 files changed, 601 insertions(+), 1273 deletions(-)
 delete mode 100644 src/shared/drivers/runcam/RuncamSerial.h
 delete mode 100644 src/shared/sensors/VN100/VN100Serial.h
 delete mode 100644 src/shared/utils/SerialInterface.h
 delete mode 100644 src/tests/drivers/test-vn100serial.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 197c8e5e5..cd569bd1c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -227,9 +227,6 @@ sbs_target(test-timer-utils stm32f429zi_stm32f4discovery)
 add_executable(test-timestamptimer src/tests/drivers/timer/test-timestamptimer.cpp)
 sbs_target(test-timestamptimer stm32f429zi_stm32f4discovery)
 
-add_executable(test-vn100serial src/tests/drivers/test-vn100serial.cpp)
-sbs_target(test-vn100serial stm32f407vg_stm32f4discovery)
-
 add_executable(test-xbee-bidir src/tests/drivers/xbee/test-xbee-bidir.cpp)
 sbs_target(test-xbee-bidir stm32f429zi_skyward_death_stack_x)
 
@@ -245,8 +242,11 @@ sbs_target(test-xbee-rcv stm32f429zi_stm32f4discovery)
 add_executable(test-xbee-snd src/tests/drivers/xbee/test-xbee-snd.cpp)
 sbs_target(test-xbee-snd stm32f429zi_stm32f4discovery)
 
-add_executable(test-usart src/tests/drivers/usart/test-usart.cpp)
-sbs_target(test-usart stm32f407vg_stm32f4discovery)
+add_executable(test-usart-f4 src/tests/drivers/usart/test-usart.cpp)
+sbs_target(test-usart-f4 stm32f429zi_stm32f4discovery)
+
+add_executable(test-usart-f7 src/tests/drivers/usart/test-usart.cpp)
+sbs_target(test-usart-f7 stm32f767zi_nucleo)
 
 add_executable(test-i2c-driver-f4 src/tests/drivers/i2c/test-i2c-driver.cpp)
 sbs_target(test-i2c-driver-f4 stm32f429zi_stm32f4discovery)
diff --git a/src/entrypoints/runcam-settings.cpp b/src/entrypoints/runcam-settings.cpp
index 770fc5f53..d87ac0056 100644
--- a/src/entrypoints/runcam-settings.cpp
+++ b/src/entrypoints/runcam-settings.cpp
@@ -52,7 +52,10 @@ int main()
 
     tx.alternateFunction(7);
     rx.alternateFunction(7);
-    Runcam test(1);
+
+    USART usart1(USART1, 115200);
+
+    Runcam test(usart1);
     if (!test.init())
     {
         return -1;
diff --git a/src/shared/drivers/runcam/Runcam.cpp b/src/shared/drivers/runcam/Runcam.cpp
index 87f14d8b9..95f51480e 100644
--- a/src/shared/drivers/runcam/Runcam.cpp
+++ b/src/shared/drivers/runcam/Runcam.cpp
@@ -27,7 +27,7 @@
 namespace Boardcore
 {
 
-Runcam::Runcam(unsigned int portNumber) : portNumber(portNumber) {}
+Runcam::Runcam(USARTInterface &usart) : usart(usart) {}
 
 bool Runcam::init()
 {
@@ -38,12 +38,6 @@ bool Runcam::init()
         return true;
     }
 
-    if (!configureSerialCommunication())
-    {
-        LOG_ERR(logger, "Unable to config camera port");
-        return false;
-    }
-
     isInit = true;
 
     Runcam::moveDown();
@@ -63,39 +57,18 @@ bool Runcam::close()
         return true;
     }
 
-    // Close the serial
-    if (!serialInterface->closeSerial())
-    {
-        LOG_ERR(logger, "Unable to close serial communication");
-        return false;
-    }
-
     isInit = false;
 
-    // Free the serialInterface memory
-    delete serialInterface;
-
     return true;
 }
 
-void Runcam::openMenu() { serialInterface->send(&OPEN_MENU, 4); }
-
-void Runcam::selectSetting() { serialInterface->send(&SELECT_SETTING, 4); }
+void Runcam::openMenu() { usart.write(&OPEN_MENU, sizeof(OPEN_MENU)); }
 
-void Runcam::moveDown() { serialInterface->send(&MOVE_DOWN, 4); }
-
-bool Runcam::configureSerialCommunication()
+void Runcam::selectSetting()
 {
-    serialInterface = new RuncamSerial(portNumber, defaultBaudRate);
-
-    // Check correct serial init
-    if (!serialInterface->init())
-    {
-        LOG_ERR(logger, "Unable to config the default serial port");
-        return false;
-    }
-
-    return true;
+    usart.write(&SELECT_SETTING, sizeof(SELECT_SETTING));
 }
 
+void Runcam::moveDown() { usart.write(&MOVE_DOWN, sizeof(MOVE_DOWN)); }
+
 }  // namespace Boardcore
diff --git a/src/shared/drivers/runcam/Runcam.h b/src/shared/drivers/runcam/Runcam.h
index eabb564ae..3095ae500 100644
--- a/src/shared/drivers/runcam/Runcam.h
+++ b/src/shared/drivers/runcam/Runcam.h
@@ -105,7 +105,7 @@ through the two modes: Video/OSD settings(Long press wifi button);
 #include <diagnostic/PrintLogger.h>
 #include <utils/Debug.h>
 
-#include "RuncamSerial.h"
+#include "drivers/usart/USART.h"
 
 namespace Boardcore
 {
@@ -116,7 +116,9 @@ namespace Boardcore
 class Runcam
 {
 public:
-    Runcam(unsigned int portNumber = defaultPortNumber);
+    explicit Runcam(USARTInterface &serial);
+
+    Runcam() = delete;
 
     bool init();
 
@@ -129,12 +131,6 @@ public:
     void moveDown();
 
 private:
-    /**
-     * @brief Configure Serial Communication
-     */
-    bool configureSerialCommunication();
-
-    unsigned int portNumber;
     bool isInit = false;
 
     /**
@@ -153,12 +149,9 @@ private:
      */
     uint32_t OPEN_MENU = 0xCC01024D;
 
-    RuncamSerial *serialInterface = nullptr;
+    USARTInterface &usart;
 
     PrintLogger logger = Logging::getLogger("runcam");
-
-    static const unsigned int defaultBaudRate   = 115200;
-    static const unsigned int defaultPortNumber = 1;
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/drivers/runcam/RuncamSerial.h b/src/shared/drivers/runcam/RuncamSerial.h
deleted file mode 100644
index bf5158e0b..000000000
--- a/src/shared/drivers/runcam/RuncamSerial.h
+++ /dev/null
@@ -1,169 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
- *
- * 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.
- */
-#pragma once
-
-#include <arch/common/drivers/serial.h>
-#include <fcntl.h>
-#include <filesystem/file_access.h>
-#include <miosix.h>
-
-namespace Boardcore
-{
-
-/**
- * @brief Class to communicate with the Runcam via serial.
- */
-class RuncamSerial
-{
-public:
-    RuncamSerial(int serialPortNumber, int serialBaudRate);
-
-    /**
-     * @brief Initialization method
-     * @return Boolean which communicates the init process result
-     */
-    bool init();
-
-    /**
-     * @brief Writes in Serial data
-     * Note: to send std::string via serial you need to pass use "c_str()"
-     * method
-     * @param Data to be sent via serial
-     * @return Boolean which communicates the send process result
-     */
-    template <typename DataSend>
-    bool send(DataSend *data, int length);
-
-    /**
-     * @brief Reads from serial
-     * @param Pointer to the data structure where we need to store data
-     * @return Boolean which communicates the recive process result
-     */
-    template <typename DataReceive>
-    bool recv(DataReceive *data, int length);
-
-    /**
-     * @brief Closes the serial communication
-     */
-    bool closeSerial();
-
-private:
-    int serialPortNumber;
-    int serialBaudRate;
-    int serialFileDescriptor = 0;
-    bool isInit              = false;
-};
-
-inline RuncamSerial::RuncamSerial(int serialPortNumber, int serialBaudRate)
-    : serialPortNumber(serialPortNumber), serialBaudRate(serialBaudRate)
-{
-}
-
-inline bool RuncamSerial::init()
-{
-    // If already initialized i avoid the procedure
-    if (isInit)
-    {
-        return true;
-    }
-
-    // Retrieve the file system instance
-    miosix::intrusive_ref_ptr<miosix::DevFs> devFs =
-        miosix::FilesystemManager::instance().getDevFs();
-
-    // Create the serial file which we read and write to communicate
-    if (!(devFs->addDevice(
-            "Runcam",
-            miosix::intrusive_ref_ptr<miosix::Device>(
-                new miosix::STM32Serial(serialPortNumber, serialBaudRate)))))
-    {
-        return false;
-    }
-
-    // Open the file
-    serialFileDescriptor = open("/dev/Runcam", O_RDWR);
-
-    // Check the descriptor
-    if (serialFileDescriptor <= 0)
-    {
-        return false;
-    }
-
-    // Success
-    isInit = true;
-    return true;
-}
-
-template <typename DataSend>
-inline bool RuncamSerial::send(DataSend *data, int length)
-{
-    // Check if the serial has been previously initialized
-    if (!isInit)
-    {
-        return false;
-    }
-
-    // Write the file with the data
-    if (!write(serialFileDescriptor, data, length))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-template <typename DataReceive>
-inline bool RuncamSerial::recv(DataReceive *data, int length)
-{
-    if (!isInit)
-    {
-        return false;
-    }
-
-    // Read the data and store it in memory
-    if (!read(serialFileDescriptor, data, length))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-inline bool RuncamSerial::closeSerial()
-{
-    // Retrieve the file system instance
-    miosix::intrusive_ref_ptr<miosix::DevFs> devFs =
-        miosix::FilesystemManager::instance().getDevFs();
-
-    // Close the file descriptor
-    close(serialFileDescriptor);
-
-    // Remove the file
-    if (!(devFs->remove("Runcam")))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-}  // namespace Boardcore
diff --git a/src/shared/drivers/usart/USART.cpp b/src/shared/drivers/usart/USART.cpp
index 00a648527..171edb219 100644
--- a/src/shared/drivers/usart/USART.cpp
+++ b/src/shared/drivers/usart/USART.cpp
@@ -32,14 +32,16 @@
 #include "filesystem/file_access.h"
 #include "miosix.h"
 
-Boardcore::USART *Boardcore::USART::ports[N_USART_PORTS];
+///< Pointer to serial port classes to let interrupts access the classes
+Boardcore::USART *ports[N_USART_PORTS];
 
+#ifdef USART1
 /**
  * \internal Interrupt routine for usart1 actual implementation.
  */
 void __attribute__((used)) usart1irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[0];
+    Boardcore::USART *port_boardcore = ports[0];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -59,13 +61,15 @@ void __attribute__((naked, used)) USART1_IRQHandler()
     asm volatile("bl _Z22usart1irqImplBoardcorev");
     restoreContext();
 }
+#endif
 
+#ifdef USART2
 /**
  * \internal Interrupt routine for usart2 actual implementation.
  */
 void __attribute__((used)) usart2irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[1];
+    Boardcore::USART *port_boardcore = ports[1];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -85,13 +89,15 @@ void __attribute__((naked, used)) USART2_IRQHandler()
     asm volatile("bl _Z22usart2irqImplBoardcorev");
     restoreContext();
 }
+#endif
 
+#ifdef USART3
 /**
  * \internal Interrupt routine for usart3 actual implementation.
  */
 void __attribute__((used)) usart3irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[2];
+    Boardcore::USART *port_boardcore = ports[2];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -111,13 +117,15 @@ void __attribute__((naked, used)) USART3_IRQHandler()
     asm volatile("bl _Z22usart3irqImplBoardcorev");
     restoreContext();
 }
+#endif
 
+#ifdef UART4
 /**
  * \internal Interrupt routine for uart4 actual implementation.
  */
 void __attribute__((used)) uart4irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[3];
+    Boardcore::USART *port_boardcore = ports[3];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -137,13 +145,15 @@ void __attribute__((naked, used)) UART4_IRQHandler()
     asm volatile("bl _Z21uart4irqImplBoardcorev");
     restoreContext();
 }
+#endif
 
+#ifdef UART5
 /**
  * \internal Interrupt routine for uart5 actual implementation.
  */
 void __attribute__((used)) uart5irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[4];
+    Boardcore::USART *port_boardcore = ports[4];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -163,13 +173,15 @@ void __attribute__((naked, used)) UART5_IRQHandler()
     asm volatile("bl _Z21uart5irqImplBoardcorev");
     restoreContext();
 }
+#endif
 
+#ifdef USART6
 /**
  * \internal Interrupt routine for usart6 actual implementation.
  */
 void __attribute__((used)) usart6irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[5];
+    Boardcore::USART *port_boardcore = ports[5];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -189,15 +201,15 @@ void __attribute__((naked, used)) USART6_IRQHandler()
     asm volatile("bl _Z22usart6irqImplBoardcorev");
     restoreContext();
 }
+#endif
 
-#ifdef STM32F429xx
-
+#ifdef UART7
 /**
  * \internal Interrupt routine for uart7 actual implementation.
  */
 void __attribute__((used)) uart7irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[6];
+    Boardcore::USART *port_boardcore = ports[6];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -217,13 +229,15 @@ void __attribute__((naked, used)) UART7_IRQHandler()
     asm volatile("bl _Z21uart7irqImplBoardcorev");
     restoreContext();
 }
+#endif
 
+#ifdef UART8
 /**
  * \internal Interrupt routine for uart8 actual implementation.
  */
 void __attribute__((used)) uart8irqImplBoardcore()
 {
-    Boardcore::USART *port_boardcore = Boardcore::USART::ports[7];
+    Boardcore::USART *port_boardcore = ports[7];
     if (port_boardcore)
         port_boardcore->IRQhandleInterrupt();
     else
@@ -243,129 +257,132 @@ void __attribute__((naked, used)) UART8_IRQHandler()
     asm volatile("bl _Z21uart8irqImplBoardcorev");
     restoreContext();
 }
-
-#endif  // STM32F429xx
+#endif
 
 namespace Boardcore
 {
 
-USARTInterface::~USARTInterface() {}
-
-void USART::IRQhandleInterrupt()
-{
-    char c;
-
-#ifndef _ARCH_CORTEXM7_STM32F7
-    // If read data register is empty then read data
-    if (usart->SR & USART_SR_RXNE)
-    {
-        // Always read data, since this clears interrupt flags
-        c = usart->DR;
-        // If no error put data in buffer
-        if (!(usart->SR & USART_SR_FE))
-            if (rxQueue.tryPut(c) == false)  // FIFO overflow
-                ;
-
-        idle = false;
-    }
-
-    if (usart->SR & USART_SR_IDLE)
-        idle = true;
-
-    if (usart->SR & USART_SR_IDLE || rxQueue.size() >= rxQueue.capacity() / 2)
-    {
-        c = usart->DR;  // Clears interrupt flags
-
-        // Enough data in buffer or idle line, awake thread
-        if (rxWaiting)
-        {
-            rxWaiting->IRQwakeup();
-            rxWaiting = 0;
-        }
-    }
-#else
-    // If read data register is empty then read data
-    if (usart->ISR & USART_ISR_RXNE)
-    {
-        // Always read data
-        c = usart->RDR;
-        // If no error put data in buffer
-        if (!(usart->ISR & USART_ISR_FE))
-            if (rxQueue.tryPut(c) == false)  // FIFO overflow
-                ;
-
-        idle = false;
-    }
-
-    if (usart->ISR & USART_ISR_IDLE)
-        idle = true;
-
-    if (usart->ISR & USART_ISR_IDLE || rxQueue.size() >= rxQueue.capacity() / 2)
-    {
-        usart->ICR = USART_ICR_IDLECF;  // Clears interrupt flags
-
-        // Enough data in buffer or idle line, awake thread
-        if (rxWaiting)
-        {
-            rxWaiting->IRQwakeup();
-            rxWaiting = 0;
-        }
-    }
-#endif
-}
-
-USART::USART(USARTType *usart, Baudrate baudrate, unsigned int queueLen)
-    : rxQueue(queueLen)
+USARTInterface::USARTInterface(USARTType *usart, int baudrate)
+    : usart(usart), baudrate(baudrate)
 {
     // Setting the id of the serial port
     switch (reinterpret_cast<uint32_t>(usart))
     {
+#ifdef USART1
         case USART1_BASE:
-            this->id = 1;
-            irqn     = USART1_IRQn;
+            this->id             = 1;
+            irqn                 = USART1_IRQn;
+            this->serialPortName = std::string("usart1");
             break;
+#endif
+#ifdef USART2
         case USART2_BASE:
-            this->id = 2;
-            irqn     = USART2_IRQn;
+            this->id             = 2;
+            irqn                 = USART2_IRQn;
+            this->serialPortName = std::string("usart2");
             break;
+#endif
+#ifdef USART3
         case USART3_BASE:
-            this->id = 3;
-            irqn     = USART3_IRQn;
+            this->id             = 3;
+            irqn                 = USART3_IRQn;
+            this->serialPortName = std::string("usart3");
             break;
+#endif
+#ifdef UART4
         case UART4_BASE:
-            this->id = 4;
-            irqn     = UART4_IRQn;
+            this->id             = 4;
+            irqn                 = UART4_IRQn;
+            this->serialPortName = std::string("uart4");
             break;
+#endif
+#ifdef UART5
         case UART5_BASE:
-            this->id = 5;
-            irqn     = UART5_IRQn;
+            this->id             = 5;
+            irqn                 = UART5_IRQn;
+            this->serialPortName = std::string("uart5");
             break;
+#endif
+#ifdef USART6
         case USART6_BASE:
-            this->id = 6;
-            irqn     = USART6_IRQn;
+            this->id             = 6;
+            irqn                 = USART6_IRQn;
+            this->serialPortName = std::string("usart6");
             break;
-#ifdef STM32F429xx
+#endif
+#ifdef UART7
         case UART7_BASE:
-            this->id = 7;
-            irqn     = UART7_IRQn;
+            this->id             = 7;
+            irqn                 = UART7_IRQn;
+            this->serialPortName = std::string("uart7");
             break;
+#endif
+#ifdef UART8
         case UART8_BASE:
-            this->id = 8;
-            irqn     = UART8_IRQn;
+            this->id             = 8;
+            irqn                 = UART8_IRQn;
+            this->serialPortName = std::string("uart8");
             break;
-#endif  // STM32F429xx
+#endif
+        default:
+            LOG_ERR(logger, "USART selected not supported!");
+            D(assert(false && "USART selected not supported!"));
     }
+}
 
-    this->usart = usart;
+USARTInterface::~USARTInterface() {}
 
-    // Enabling the peripheral on the right APB
-    ClockUtils::enablePeripheralClock(usart);
+void USART::IRQhandleInterrupt()
+{
+    char c;
+    bool received = false;
+    bool framingError;
 
-    // Enabling the usart peripheral
+#ifndef _ARCH_CORTEXM7_STM32F7
+    // If read data register is empty then read data
+    received = ((usart->SR & USART_SR_RXNE) == 0 ? false : true);
+    // If no error put data in buffer
+    framingError = ((usart->SR & USART_SR_FE) == 0 ? false : true);
+    idle         = ((usart->SR & USART_SR_IDLE) == 0 ? false : true);
+    // Always read data, since this clears interrupt flags
+    c = usart->DR;
+#else
+    // If read data register is empty then read data
+    received = ((usart->ISR & USART_ISR_RXNE) == 0 ? false : true);
+    // If no error put data in buffer
+    framingError = ((usart->ISR & USART_ISR_FE) == 0 ? false : true);
+    idle         = ((usart->ISR & USART_ISR_IDLE) == 0 ? false : true);
+    // Clears interrupt flags
+    usart->ICR = USART_ICR_IDLECF;
+    // Always read data, since this clears interrupt flags
+    c = usart->RDR;
+#endif
+
+    // If we received some data without framing error but the tryPut failed,
+    // report a FIFO overflow
+    if (framingError || (received && !rxQueue.tryPut(c)))
     {
-        miosix::FastInterruptDisableLock dLock;
-        usart->CR1 |= USART_CR1_UE;
+        error = true;
+    }
+
+    // Wake up thread if communication finished (idle state), buffer reached
+    // half of his capacity or error occurred
+    if (error || idle || (rxQueue.size() >= rxQueue.capacity() / 2))
+    {
+        // Enough data in buffer or idle line, awake thread
+        if (rxWaiting)
+        {
+            rxWaiting->IRQwakeup();
+            rxWaiting = nullptr;
+        }
     }
+}
+
+USART::USART(USARTType *usart, int baudrate, unsigned int queueLen)
+    : USARTInterface(usart, baudrate), rxQueue(queueLen)
+{
+    // Enabling the peripheral on the right APB
+    ClockUtils::enablePeripheralClock(usart);
 
     // Setting the baudrate chosen
     setBaudrate(baudrate);
@@ -375,60 +392,48 @@ USART::USART(USARTType *usart, Baudrate baudrate, unsigned int queueLen)
     setWordLength(USART::WordLength::BIT8);
     setParity(USART::ParityBit::NO_PARITY);
     setOversampling(false);
-}
-
-USART::~USART()
-{
-    miosix::FastInterruptDisableLock dLock;
-
-    // Take out the usart object we are going to destruct
-    USART::ports[this->id - 1] = nullptr;
-
-    // Disabling the usart
-    usart->CR1 &= ~(USART_CR1_UE | USART_CR1_TE | USART_CR1_RE);
-
-    // Disabling the interrupt of the serial port
-    NVIC_DisableIRQ(irqn);
-}
-
-bool USART::init()
-{
-    if (id < 1 || id > MAX_SERIAL_PORTS)
-    {
-        TRACE("Not supported USART id\n");
-        return false;
-    }
 
     {
         miosix::FastInterruptDisableLock dLock;
 
         // Enable usart, receiver, receiver interrupt and idle interrupt
-        usart->CR1 |= USART_CR1_RXNEIE    // Interrupt on data received
+        usart->CR1 |= USART_CR1_UE        // Enabling the uart peripheral
+                      | USART_CR1_RXNEIE  // Interrupt on data received
                       | USART_CR1_IDLEIE  // interrupt on idle line
                       | USART_CR1_TE      // Transmission enabled
                       | USART_CR1_RE;     // Reception enabled
 
         // Sample only one bit
         usart->CR3 |= USART_CR3_ONEBIT;
+    }
 
-        // Enabling the interrupt for the relative serial port
-        NVIC_SetPriority(irqn, 15);
-        NVIC_EnableIRQ(irqn);
+    // Add to the array of usarts so that the interrupts can see it
+    ports[id - 1] = this;
 
-        // Add to the array of usarts so that the interrupts can see it
-        USART::ports[id - 1] = this;
-    }
+    // Enabling the interrupt for the relative serial port
+    NVIC_SetPriority(irqn, 15);
+    NVIC_EnableIRQ(irqn);
 
     // Clearing the queue for random data read at the beginning
-    miosix::Thread::sleep(1);
     this->clearQueue();
+}
 
-    return true;
+USART::~USART()
+{
+    miosix::FastInterruptDisableLock dLock;
+
+    // Take out the usart object we are going to destruct
+    ports[this->id - 1] = nullptr;
+
+    // Disabling the usart
+    usart->CR1 &= ~(USART_CR1_UE | USART_CR1_TE | USART_CR1_RE);
+
+    // Disabling the interrupt of the serial port
+    NVIC_DisableIRQ(irqn);
 }
 
 void USART::setWordLength(WordLength wordLength)
 {
-
     miosix::FastInterruptDisableLock dLock;
     (wordLength == WordLength::BIT8 ? usart->CR1 &= ~USART_CR1_M
                                     : usart->CR1 |= USART_CR1_M);
@@ -462,7 +467,7 @@ void USART::setOversampling(bool oversampling)
                   : usart->CR1 &= ~USART_CR1_OVER8);
 }
 
-void USART::setBaudrate(Baudrate baudrate)
+void USART::setBaudrate(int baudrate)
 {
     /*
      * Baudrate setting:
@@ -475,55 +480,49 @@ void USART::setBaudrate(Baudrate baudrate)
      */
     miosix::InterruptDisableLock dLock;
 
-    /*
-     * TODO: This is modified only for compatibility with the past, MUST check
-     * the clock settings in order to use the right method
-     * 'getAPBPeripheralsClock()'
-     */
     // USART1 and USART6 are always connected to the APB2, while all the others
     // UART/USART peripherals are always connected to APB1
-    uint32_t f = ClockUtils::getAPBTimersClock(
+    uint32_t f = ClockUtils::getAPBPeripheralsClock(
         (id == 1 || id == 6 ? ClockUtils::APB::APB2     // High speed APB2
                             : ClockUtils::APB::APB1));  // Low speed APB1
 
     // <<4 in order to shift to left of 4 positions, to create a fixed point
     // number of 4 decimal digits /8 == >>3 in order to divide per 8 (from the
-    // formula in the datasheet)
-    uint32_t USART_DIV = ((f << 1) / (((int)baudrate * (over8 ? 1 : 2))));
+    // formula in the datasheet). So it should be << 1 but, in order to make the
+    // approximation later, we shift it of 2 positions
+    uint32_t brr = ((f << 2) / (((int)baudrate * (over8 ? 1 : 2))));
 
     // rounding to the nearest
-    uint32_t brr = (USART_DIV / 2) + (USART_DIV & 1);
-
-    if (over8)
-    {
-        brr += brr & 1;
-    }
-
-    usart->BRR = brr;
+    usart->BRR = (brr / 2) + (brr & 1);
 
     this->baudrate = baudrate;
 }
 
-int USART::read(void *buffer, size_t nBytes)
+bool USART::readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
+                     const bool blocking)
 {
     miosix::Lock<miosix::FastMutex> l(rxMutex);
 
     char *buf     = reinterpret_cast<char *>(buffer);
     size_t result = 0;
+    error         = false;
     miosix::FastInterruptDisableLock dLock;
     for (;;)
     {
-        // Try to get data from the queue
+        // Try to get all the data possible from the queue
         for (; result < nBytes; result++)
         {
-            if (rxQueue.tryGet(buf[result]) == false)
+            if (!rxQueue.tryGet(buf[result]))
                 break;
+
             // This is here just not to keep IRQ disabled for the whole loop
             miosix::FastInterruptEnableLock eLock(dLock);
         }
 
-        // Not checking if the nBytes read are more than 0
-        if (result == nBytes || (idle && result > 0))
+        // If blocking, we are waiting for at least one byte of data before
+        // returning. If not blocking, in the case the bus is idle we return
+        // anyway
+        if ((result == nBytes) || (idle && (!blocking || (result > 0))))
             break;
 
         // Wait for data in the queue
@@ -537,11 +536,12 @@ int USART::read(void *buffer, size_t nBytes)
             }
         } while (rxWaiting);
     }
+    nBytesRead = result;
 
-    return result;
+    return (result > 0);
 }
 
-int USART::write(void *buffer, size_t nBytes)
+void USART::write(const void *buffer, size_t nBytes)
 {
     miosix::Lock<miosix::FastMutex> l(txMutex);
 
@@ -561,11 +561,9 @@ int USART::write(void *buffer, size_t nBytes)
         usart->TDR = *buf++;
 #endif
     }
-
-    return i;
 }
 
-int USART::writeString(const char *buffer)
+void USART::writeString(const char *buffer)
 {
     int i = 0;
     miosix::Lock<miosix::FastMutex> l(txMutex);
@@ -595,75 +593,55 @@ int USART::writeString(const char *buffer)
 
         i++;
     };
-
-    return i;
 }
 
 void USART::clearQueue() { rxQueue.reset(); }
 
-STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate)
+STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, int baudrate)
+    : USARTInterface(usart, baudrate)
 {
-    this->usart    = usart;
-    this->baudrate = baudrate;
-    switch (reinterpret_cast<uint32_t>(usart))
+    if (this->id < 1 || this->id > 4)
     {
-        case USART1_BASE:
-            this->id = 1;
-            initPins(u1tx1::getPin(), 7, u1rx1::getPin(), 7);
-            this->serialPortName = std::string("usart1");
-            break;
-        case USART2_BASE:
-            this->id = 2;
-            initPins(u2tx1::getPin(), 7, u2rx1::getPin(), 7);
-            this->serialPortName = std::string("usart2");
-            break;
-        case USART3_BASE:
-            this->id = 3;
-            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;
+        LOG_ERR(logger, "USART selected not supported for STM32SerialWrapper!");
+        D(assert(false &&
+                 "USART selected not supported for STM32SerialWrapper!"));
+    }
+
+    // Creates and adds the serial port to the devices
+    this->serial = new miosix::STM32Serial(id, baudrate);
+
+    if (!serialCommSetup())
+    {
+        LOG_ERR(logger,
+                "[STM32SerialWrapper] can't initialize serial communication!");
+        D(assert(false &&
+                 "[STM32SerialWrapper] Error : can't initialize serial "
+                 "communication!\n"));
     }
-    initialized = false;
-    fd          = -1;
 }
 
-STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate,
+STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, int baudrate,
                                        miosix::GpioPin tx, miosix::GpioPin rx)
+    : USARTInterface(usart, baudrate)
 {
-    this->usart    = usart;
-    this->baudrate = baudrate;
-    switch (reinterpret_cast<uint32_t>(usart))
+    if (this->id < 1 || this->id > 4)
     {
-        case USART1_BASE:
-            this->id             = 1;
-            this->serialPortName = std::string("usart1");
-            break;
-        case USART2_BASE:
-            this->id             = 2;
-            this->serialPortName = std::string("usart2");
-            break;
-        case USART3_BASE:
-            this->id             = 3;
-            this->serialPortName = std::string("usart3");
-            break;
-        case UART4_BASE:
-            this->id             = 4;
-            this->serialPortName = std::string("uart4");
-            break;
+        LOG_ERR(logger, "USART selected not supported for STM32SerialWrapper!");
+        D(assert(false &&
+                 "USART selected not supported for STM32SerialWrapper!"));
     }
 
-    if (id < 4)
-        initPins(tx, 7, rx, 7);
-    else
-        initPins(tx, 8, rx, 8);
+    // Creates and adds the serial port to the devices
+    this->serial = new miosix::STM32Serial(id, baudrate, tx, rx);
 
-    initialized = false;
-    fd          = -1;
+    if (!serialCommSetup())
+    {
+        LOG_ERR(logger,
+                "[STM32SerialWrapper] can't initialize serial communication!");
+        D(assert(false &&
+                 "[STM32SerialWrapper] Error : can't initialize serial "
+                 "communication!\n"));
+    }
 }
 
 STM32SerialWrapper::~STM32SerialWrapper()
@@ -674,50 +652,15 @@ STM32SerialWrapper::~STM32SerialWrapper()
     devFs->remove(serialPortName.c_str());
 }
 
-bool STM32SerialWrapper::init()
-{
-    if (id > 4)
-    {
-        TRACE(
-            "[STM32SerialWrapper] USART id greater than 3 is not supported\n");
-        return false;
-    }
-
-    if (initialized)
-    {
-        TRACE(
-            "[STM32SerialWrapper] Error : serial communication already "
-            "initialized!\n");
-        return false;
-    }
-    else if (!serialCommSetup())
-    {
-        TRACE(
-            "[STM32SerialWrapper] Error : can't initialize serial "
-            "communication!\n");
-        return false;
-    }
-
-    initialized = true;
-    return true;
-}
-
 bool STM32SerialWrapper::serialCommSetup()
 {
-    // Creates and adds the serial port to the devices
-    if (!pinInitialized)
-        serial = new miosix::STM32Serial(id, static_cast<int>(baudrate));
-    else
-    {
-        serial =
-            new miosix::STM32Serial(id, static_cast<int>(baudrate), tx, rx);
-    }
-
     // Adds a device to the file system
     if (!miosix::FilesystemManager::instance().getDevFs()->addDevice(
             serialPortName.c_str(),
             miosix::intrusive_ref_ptr<miosix::Device>(serial)))
+    {
         return false;
+    }
 
     // Path string "/dev/<name_of_port>" for the port we want to open
     std::string serialPortPath = "/dev/" + serialPortName;
@@ -734,41 +677,33 @@ bool STM32SerialWrapper::serialCommSetup()
     return true;
 }
 
-bool STM32SerialWrapper::initPins(miosix::GpioPin tx, int nAFtx,
-                                  miosix::GpioPin rx, int nAFrx)
+bool STM32SerialWrapper::readImpl(void *buffer, size_t nBytes,
+                                  size_t &nBytesRead, const bool blocking)
 {
-    if (pinInitialized)
-        return false;
-
-    miosix::FastInterruptDisableLock dLock;
-
-    this->tx = tx;
-    this->rx = rx;
-
-    tx.mode(miosix::Mode::ALTERNATE);
-    tx.alternateFunction(nAFtx);
-
-    rx.mode(miosix::Mode::ALTERNATE);
-    rx.alternateFunction(nAFrx);
+    // non-blocking read not supported in STM32SerialWrapper
+    if (!blocking)
+    {
+        LOG_ERR(logger,
+                "STM32SerialWrapper::read doesn't support non-blocking read");
+        D(assert(false &&
+                 "STM32SerialWrapper::read doesn't support non-blocking read"));
+    }
 
-    pinInitialized = true;
-    return true;
-}
+    size_t n   = ::read(fd, buffer, nBytes);
+    nBytesRead = n;
 
-int STM32SerialWrapper::writeString(const char *data)
-{
-    // strlen + 1 in order to send the '/0' terminated string
-    return ::write(fd, data, strlen(data) + 1);
+    return (n > 0);
 }
 
-int STM32SerialWrapper::write(void *buf, size_t nChars)
+void STM32SerialWrapper::write(const void *buffer, size_t nBytes)
 {
-    return ::write(fd, buf, nChars);
+    ::write(fd, buffer, nBytes);
 }
 
-int STM32SerialWrapper::read(void *buf, size_t nBytes)
+void STM32SerialWrapper::writeString(const char *buffer)
 {
-    return ::read(fd, buf, nBytes);
+    // strlen + 1 in order to send the '/0' terminated string
+    ::write(fd, buffer, strlen(buffer) + 1);
 }
 
 }  // namespace Boardcore
diff --git a/src/shared/drivers/usart/USART.h b/src/shared/drivers/usart/USART.h
index 8ba3a9369..0e45b41a4 100644
--- a/src/shared/drivers/usart/USART.h
+++ b/src/shared/drivers/usart/USART.h
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <diagnostic/PrintLogger.h>
 #include <interfaces/arch_registers.h>
 #include <miosix.h>
 #include <utils/ClockUtils.h>
@@ -37,69 +38,26 @@ using USARTType = USART_TypeDef;
 using USARTType = USART_TypeDef;
 #endif
 
-#ifdef STM32F429xx
+#if defined(UART8)
 #define N_USART_PORTS 8
-#else
+#elif defined(UART7)
+#define N_USART_PORTS 7
+#elif defined(USART6)
 #define N_USART_PORTS 6
+#elif defined(UART5)
+#define N_USART_PORTS 5
+#elif defined(UART4)
+#define N_USART_PORTS 4
+#elif defined(USART3)
+#define N_USART_PORTS 3
+#elif defined(USART2)
+#define N_USART_PORTS 2
+#elif defined(USART1)
+#define N_USART_PORTS 1
+#else
+#error "Your architecture doesn't support UART"
 #endif
 
-// A nice feature of the stm32 is that the USART are connected to the same
-// GPIOS in all families, stm32f1, f2, f4 and l1. Additionally, USART1 and
-// USART6 are always connected to the APB2, while the other USART/UARTs are
-// connected to the APB1.
-
-// USART1: AF7
-typedef miosix::Gpio<GPIOB_BASE, 6> u1tx1;
-typedef miosix::Gpio<GPIOB_BASE, 7> u1rx1;
-typedef miosix::Gpio<GPIOA_BASE, 9> u1tx2;
-typedef miosix::Gpio<GPIOA_BASE, 10> u1rx2;
-// typedef miosix::Gpio<GPIOA_BASE, 11> u1cts;
-// typedef miosix::Gpio<GPIOA_BASE, 12> u1rts;
-
-// USART2: AF7
-typedef miosix::Gpio<GPIOA_BASE, 2> u2tx1;
-typedef miosix::Gpio<GPIOA_BASE, 3> u2rx1;
-typedef miosix::Gpio<GPIOD_BASE, 5> u2tx2;
-typedef miosix::Gpio<GPIOD_BASE, 6> u2rx2;
-// typedef miosix::Gpio<GPIOA_BASE, 0> u2cts;
-// typedef miosix::Gpio<GPIOA_BASE, 1> u2rts;
-
-// USART3: AF7
-typedef miosix::Gpio<GPIOB_BASE, 10> u3tx1;
-typedef miosix::Gpio<GPIOB_BASE, 11> u3rx1;
-typedef miosix::Gpio<GPIOD_BASE, 8> u3tx2;
-typedef miosix::Gpio<GPIOD_BASE, 9> u3rx2;
-// typedef miosix::Gpio<GPIOB_BASE, 13> u3cts;
-// typedef miosix::Gpio<GPIOB_BASE, 14> u3rts;
-
-// UART4: AF8
-typedef miosix::Gpio<GPIOA_BASE, 0> u4tx1;
-typedef miosix::Gpio<GPIOA_BASE, 1> u4rx1;
-typedef miosix::Gpio<GPIOC_BASE, 10> u4tx2;
-typedef miosix::Gpio<GPIOC_BASE, 11> u4rx2;
-
-// UART5: AF8
-typedef miosix::Gpio<GPIOC_BASE, 12> u5tx;
-typedef miosix::Gpio<GPIOD_BASE, 2> u5rx;
-
-// USART6: AF8
-typedef miosix::Gpio<GPIOC_BASE, 6> u6tx1;
-typedef miosix::Gpio<GPIOC_BASE, 7> u6rx1;
-#ifdef STM32F429xx
-typedef miosix::Gpio<GPIOG_BASE, 14> u6tx2;
-typedef miosix::Gpio<GPIOG_BASE, 9> u6rx2;
-
-// USART7: AF8
-typedef miosix::Gpio<GPIOE_BASE, 8> u7tx1;
-typedef miosix::Gpio<GPIOE_BASE, 7> u7rx1;
-typedef miosix::Gpio<GPIOF_BASE, 7> u7tx2;
-typedef miosix::Gpio<GPIOF_BASE, 6> u7rx2;
-
-// USART8: AF8
-typedef miosix::Gpio<GPIOE_BASE, 1> u8tx;
-typedef miosix::Gpio<GPIOE_BASE, 0> u8rx;
-#endif  // STM32F429xx
-
 namespace Boardcore
 {
 
@@ -110,47 +68,55 @@ namespace Boardcore
 class USARTInterface
 {
 public:
-    enum class Baudrate : int
-    {
-        // B1200   = 1200, // NOT WORKING WITH 1200 baud
-        B2400   = 2400,
-        B9600   = 9600,
-        B19200  = 19200,
-        B38400  = 38400,
-        B57600  = 57600,
-        B115200 = 115200,
-        B230400 = 230400,
-        B256000 = 256000,
-        B460800 = 460800,
-        B921600 = 921600
-    };
+    /**
+     * @brief Constructor of the USART in order to assign usart and baudrate.
+     * @param usart Pointer to the USART interface.
+     * @param baudrate Baudrate in bit per second. Default values are [2400,
+     * 9600, 19200, 38400, 57600, 115200, 230400, 256000, 460800, 921600]
+     */
+    explicit USARTInterface(USARTType *usart, int baudrate);
 
     virtual ~USARTInterface() = 0;
 
     /**
-     * @brief Initializes the peripheral enabling his interrupts, the interrupts
-     * in the NVIC.
-     *
-     * All the setup phase (with the setting of the pins and their alternate
-     * functions) must be done before the initialization of the peripheral.
+     * @brief Blocking read operation to read nBytes or till the data transfer
+     * is complete.
+     * @param buffer Buffer that will contain the received data.
+     * @param nBytes Maximum size of the buffer.
+     * @return If operation succeeded.
      */
-    virtual bool init() = 0;
+    [[nodiscard]] virtual bool readBlocking(void *buffer, size_t nBytes)
+    {
+        size_t temp;
+        return readImpl(buffer, nBytes, temp, true);
+    };
 
     /**
      * @brief Blocking read operation to read nBytes or till the data transfer
      * is complete.
+     * @param buffer Buffer that will contain the received data.
+     * @param nBytes Maximum size of the buffer.
+     * @param nBytesRead Number of bytes read in the transaction.
+     * @return If operation succeeded.
      */
-    virtual int read(void *buffer, size_t nBytes) = 0;
+    [[nodiscard]] virtual bool readBlocking(void *buffer, size_t nBytes,
+                                            size_t &nBytesRead)
+    {
+        return readImpl(buffer, nBytes, nBytesRead, true);
+    };
 
     /**
      * @brief Blocking write operation.
+     * @param buffer Buffer that contains the data to be sent.
+     * @param nBytes Bytes to be sent.
      */
-    virtual int write(void *buf, size_t nChars) = 0;
+    virtual void write(const void *buf, size_t nBytes) = 0;
 
     /**
      * @brief Write a string to the serial, comprising the '\0' character.
+     * @param buffer Buffer that contains the string to be sent.
      */
-    virtual int writeString(const char *buffer) = 0;
+    virtual void writeString(const char *buffer) = 0;
 
     /**
      * @brief Returns the id of the serial.
@@ -158,13 +124,29 @@ public:
     int getId() { return id; };
 
 protected:
-    miosix::GpioPin tx{GPIOA_BASE, 0};
-    miosix::GpioPin rx{GPIOA_BASE, 0};
+    /**
+     * @brief Read method implementation that supports both blocking and
+     * non-blocking mode and the return of the number of bytes read.
+     * @param buffer Buffer that will contain the received data.
+     * @param nBytes Maximum size of the buffer.
+     * @param nBytesRead Number of bytes read.
+     * @param blocking Whether the read should block or not; in case it isn't
+     * blocking the read could return also 0 bytes.
+     * @return If operation succeeded.
+     */
+    virtual bool readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
+                          const bool blocking) = 0;
 
     USARTType *usart;
-    int id           = -1;  ///< Can be from 1 to 8, -1 is invalid
-    bool initialized = false;
-    Baudrate baudrate;  ///< Baudrate of the serial communication
+    int id = -1;                 ///< Can be from 1 to 8, -1 is invalid.
+    IRQn_Type irqn;              ///< IRQ number
+    std::string serialPortName;  ///< Port name of the port that has to be
+                                 ///< created for the communication
+    int baudrate;  ///< Baudrate of the serial communication; standard ones
+                   ///< are [2400, 9600, 19200, 38400, 57600, 115200,
+                   ///< 230400, 256000, 460800, 921600]
+
+    PrintLogger logger = Logging::getLogger("usart");
 };
 
 /**
@@ -188,9 +170,6 @@ public:
         PARITY    = 1
     };
 
-    ///< Pointer to serial port classes to let interrupts access the classes
-    static USART *ports[];
-
     /**
      * @brief Interrupt handler that deals with receive and idle interrupts.
      *
@@ -208,12 +187,18 @@ public:
      *
      * @param usart structure that represents the usart peripheral [accepted
      * are: USART1, USART2, USART3, UART4, UART5, USART6, UART7, UART8].
-     * @param baudrate member of the enum Baudrate that represents the baudrate
-     * with which the communication will take place.
+     * @param baudrate Baudrate in bit per second. Default values are [2400,
+     * 9600, 19200, 38400, 57600, 115200, 230400, 256000, 460800, 921600]
      */
-    USART(USARTType *usart, Baudrate baudrate,
+    USART(USARTType *usart, int baudrate,
           unsigned int queueLen = usart_queue_default_capacity);
 
+    ///< Delete copy/move constructors/operators.
+    USART(const USART &)            = delete;
+    USART &operator=(const USART &) = delete;
+    USART(USART &&)                 = delete;
+    USART &operator=(USART &&)      = delete;
+
     /**
      * @brief Disables the flags for the generation of the interrupts, the IRQ
      * from the NVIC, the peripheral and removes his pointer from the ports
@@ -222,30 +207,47 @@ public:
     ~USART() override;
 
     /**
-     * @brief Initializes the peripheral enabling his interrupts, the interrupts
-     * in the NVIC and setting the pins with the appropriate alternate
-     * functions.
+     * @brief Non-blocking read operation to read nBytes or till the data
+     * transfer is complete.
+     * @warning could also read 0 bytes.
      *
-     * All the setup phase must be done before the initialization of the
-     * peripheral. The pins must be initialized before calling this function.
+     * @param buffer Buffer that will contain the received data.
+     * @param nBytes Maximum size of the buffer.
+     * @return If operation succeeded.
      */
-    bool init() override;
+    [[nodiscard]] bool read(void *buffer, size_t nBytes)
+    {
+        size_t temp;
+        return readImpl(buffer, nBytes, temp, false);
+    }
 
     /**
-     * @brief Blocking read operation to read nBytes or till the data transfer
-     * is complete.
+     * @brief Non-blocking read operation to read nBytes or till the data
+     * transfer is complete.
+     * @warning could also read 0 bytes.
+     *
+     * @param buffer Buffer that will contain the received data.
+     * @param nBytes Maximum size of the buffer.
+     * @param nBytesRead Number of bytes read.
+     * @return If operation succeeded.
      */
-    int read(void *buffer, size_t nBytes) override;
+    [[nodiscard]] bool read(void *buffer, size_t nBytes, size_t &nBytesRead)
+    {
+        return readImpl(buffer, nBytes, nBytesRead, false);
+    };
 
     /**
      * @brief Blocking write operation.
+     * @param buffer Buffer that contains the data to be sent.
+     * @param nBytes Bytes to be sent.
      */
-    int write(void *buf, size_t nChars) override;
+    void write(const void *buf, size_t nBytes);
 
     /**
      * @brief Write a string to the serial, comprising the '\0' character.
+     * @param buffer Buffer that contains the string to be sent.
      */
-    int writeString(const char *buffer) override;
+    void writeString(const char *buffer);
 
     /**
      * @brief Set the length of the word to 8 or to 9.
@@ -272,9 +274,10 @@ public:
     /**
      * @brief Set the baudrate in the BRR register.
      *
-     * @param pb Baudrate element that represents the baudrate.
+     * @param baudrate Baudrate in bit per second. Default values are [2400,
+     * 9600, 19200, 38400, 57600, 115200, 230400, 256000, 460800, 921600]
      */
-    void setBaudrate(Baudrate br);
+    void setBaudrate(int baudrate);
 
     /**
      * @brief Sets the Over8 bit.
@@ -290,7 +293,19 @@ public:
     void clearQueue();
 
 private:
-    IRQn_Type irqn;
+    /**
+     * @brief Read method implementation that supports both
+     * blocking/non-blocking mode and the return of the number of bytes read.
+     * @param buffer Buffer that will contain the received data.
+     * @param nBytes Maximum size of the buffer.
+     * @param nBytesRead Number of bytes read.
+     * @param blocking Whether the read should block or not; in case it isn't
+     * blocking the read could return also 0 bytes.
+     * @return If operation succeeded.
+     */
+    [[nodiscard]] bool readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
+                                const bool blocking) override;
+
     miosix::FastMutex rxMutex;  ///< mutex for receiving on serial
     miosix::FastMutex txMutex;  ///< mutex for transmitting on serial
 
@@ -303,6 +318,7 @@ private:
     WordLength wordLength = WordLength::BIT8;
     int stopBits          = 1;      ///< Number of stop bits [1,2]
     bool over8            = false;  ///< Oversalmpling 8 bit
+    bool error            = false;  ///< Error occurred while receiving message
 
     ///< Default queue length
     const static unsigned int usart_queue_default_capacity = 256;
@@ -321,64 +337,63 @@ public:
      * - USART2: tx=PA2  rx=PA3
      * - USART3: tx=PB10 rx=PB11
      * @param usart structure that represents the usart peripheral [accepted
-     * are: USART1, USART2, USART3].
-     * @param baudrate member of the enum Baudrate that represents the baudrate
-     * with which the communication will take place.
+     * are: USART1, USART2, USART3, UART4].
+     * @param baudrate baudrate in bit per second. Default values are [2400,
+     * 9600, 19200, 38400, 57600, 115200, 230400, 256000, 460800, 921600]
      */
-    STM32SerialWrapper(USARTType *usart, Baudrate baudrate);
+    STM32SerialWrapper(USARTType *usart, int baudrate);
 
     /**
      * @brief Initializes the serialPortName and initializes the serial port
      * using custom pins.
      * @param usart structure that represents the usart peripheral [accepted
-     * are: USART1, USART2, USART3].
-     * @param baudrate member of the enum Baudrate that represents the baudrate
-     * with which the communication will take place.
+     * are: USART1, USART2, USART3, UART4].
+     * @param baudrate baudrate in bit per second. Default values are [2400,
+     * 9600, 19200, 38400, 57600, 115200, 230400, 256000, 460800, 921600]
      * @param tx Tranmission pin
      * @param rx Reception pin
      */
-    STM32SerialWrapper(USARTType *usart, Baudrate baudrate, miosix::GpioPin tx,
+    STM32SerialWrapper(USARTType *usart, int baudrate, miosix::GpioPin tx,
                        miosix::GpioPin rx);
 
+    ///< Delete copy/move constructors/operators.
+    STM32SerialWrapper(const STM32SerialWrapper &)            = delete;
+    STM32SerialWrapper &operator=(const STM32SerialWrapper &) = delete;
+    STM32SerialWrapper(STM32SerialWrapper &&)                 = delete;
+    STM32SerialWrapper &operator=(STM32SerialWrapper &&)      = delete;
+
     /**
      * @brief Removes the device from the list of the devices and closes the
      * file of the device.
      */
     ~STM32SerialWrapper();
 
-    /**
-     * @brief Initializes the peripheral.
-     *
-     * @see{STM32SerialWrapper::serialCommSetup}
-     */
-    bool init();
-
-    /**
-     * @brief Blocking read operation to read nBytes or till the data transfer
-     * is complete.
-     */
-    int read(void *buffer, size_t nBytes);
-
     /**
      * @brief Blocking write operation.
+     * @param buffer Buffer that contains the data to be sent.
+     * @param nBytes Bytes to be sent.
      */
-    int write(void *buf, size_t nChars);
+    void write(const void *buf, size_t nBytes);
 
     /**
      * @brief Write a string to the serial, comprising the '\0' character.
+     * @param buffer Buffer that contains the string to be sent.
      */
-    int writeString(const char *buffer);
+    void writeString(const char *buffer);
 
 private:
     /**
-     * @brief Initializes the pins with the appropriate alternate functions.
-     *
-     * @param tx Tranmission pin.
-     * @param nAFtx Tranmission pin alternate function.
-     * @param rx Reception pin.
-     * @param nAFrx Reception pin alternate function.
+     * @brief Read method implementation that supports both
+     * blocking/non-blocking mode and the return of the number of bytes read.
+     * @param buffer Buffer that will contain the received data.
+     * @param nBytes Maximum size of the buffer.
+     * @param nBytesRead Number of bytes read.
+     * @param blocking Whether the read should block or not; in case it isn't
+     * blocking the read could return also 0 bytes.
+     * @return If operation succeeded.
      */
-    bool initPins(miosix::GpioPin tx, int nAFtx, miosix::GpioPin rx, int nAFrx);
+    [[nodiscard]] bool readImpl(void *buffer, size_t nBytes, size_t &nBytesRead,
+                                const bool blocking) override;
 
     /**
      * @brief Creates a device that represents the serial port, adds it to the
@@ -386,14 +401,8 @@ private:
      */
     bool serialCommSetup();
 
-    ///< True if initPins() already called successfully, false otherwise
-    bool pinInitialized = false;
-
     miosix::STM32Serial *serial;  ///< Pointer to the serial object
 
-    //< Port name of the port that has to be created for the communication
-    std::string serialPortName;
-
     ///< File descriptor of the serial port file opened for transmission
     int fd;
 };
diff --git a/src/shared/radio/SerialTransceiver/SerialTransceiver.h b/src/shared/radio/SerialTransceiver/SerialTransceiver.h
index f5e0ae00b..2618524dc 100644
--- a/src/shared/radio/SerialTransceiver/SerialTransceiver.h
+++ b/src/shared/radio/SerialTransceiver/SerialTransceiver.h
@@ -44,7 +44,7 @@ public:
 
     ssize_t receive(uint8_t* packet, size_t packetLength)
     {
-        return usart.read(packet, packetLength);
+        return usart.readBlocking(packet, packetLength);
     }
 
 private:
diff --git a/src/shared/sensors/MBLoadCell/MBLoadCell.cpp b/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
index 7f943cd5c..6fd07f1dd 100644
--- a/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
+++ b/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
@@ -33,31 +33,15 @@ using ctrlPin2 = miosix::Gpio<GPIOC_BASE, 2>;  ///< Control R/W pin 2
 namespace Boardcore
 {
 
-MBLoadCell::MBLoadCell(LoadCellModes mode, int serialPortNum,
-                       int baudrate = 2400)
+MBLoadCell::MBLoadCell(USARTInterface &serial, LoadCellModes mode)
+    : serial(serial)
 {
     this->settings.mode = mode;
     maxPrint = maxSetted = minPrint = minSetted = 0;
-
-    // Creating the instance of the serial interface
-    serial = new SerialInterface(baudrate, serialPortNum);
 }
 
 bool MBLoadCell::init()
 {
-    if (serial->isInit())
-    {
-        lastError = SensorErrors::ALREADY_INIT;
-    }
-
-    // Initializing the serial connection
-    if (!serial->init())
-    {
-        lastError = SensorErrors::INIT_FAIL;
-        TRACE("[MBLoadCell] init of the serial communication failed\n");
-        return false;
-    }
-
     {
         // Disabling interrupts in order to set with no problems the ctrl pins
         miosix::FastInterruptDisableLock dLock;
@@ -288,12 +272,12 @@ void MBLoadCell::generateRequest(DataAsciiRequest &req,
     req.setChecksum();
 }
 
-void MBLoadCell::transmitASCII(std::string buf)
+void MBLoadCell::transmitASCII(const std::string &buf)
 {
     // Setting both the control pins to high in order to transmit
     ctrlPin1::high();
     ctrlPin2::high();
-    serial->sendString(buf);
+    serial.writeString(buf.c_str());
     miosix::Thread::sleep(10);  // Needs some time (>5ms) idk why
 }
 
@@ -304,7 +288,7 @@ std::string MBLoadCell::receiveASCII()
     // Setting both the control pins to low in order to receive
     ctrlPin1::low();
     ctrlPin2::low();
-    int len  = serial->recvString(buf, 64);
+    int len  = serial.readBlocking(buf, 64);
     buf[len] = '\0';
 
     return std::string(buf);
@@ -316,7 +300,7 @@ void MBLoadCell::receive(T *buf)
     // Setting both the control pins to low in order to receive
     ctrlPin1::low();
     ctrlPin2::low();
-    serial->recvData(buf);
+    serial.readBlocking(buf, sizeof(buf));
 }
 
 }  // namespace Boardcore
diff --git a/src/shared/sensors/MBLoadCell/MBLoadCell.h b/src/shared/sensors/MBLoadCell/MBLoadCell.h
index d8e57f5f6..1eb6fcdba 100644
--- a/src/shared/sensors/MBLoadCell/MBLoadCell.h
+++ b/src/shared/sensors/MBLoadCell/MBLoadCell.h
@@ -29,8 +29,8 @@
 #include <string>
 
 #include "MBLoadCellData.h"
+#include "drivers/usart/USART.h"
 #include "miosix.h"
-#include "utils/SerialInterface.h"
 
 namespace Boardcore
 {
@@ -54,10 +54,9 @@ public:
      * @brief constructor that initializes the serial communication with the
      * load cell
      * @param mode the mode in which the load cell is in
-     * @param serialPortNum port number to which the load cell is connected
-     * @param baudrate baudrate set on the TLB converter
+     * @param serial the serial port to be used
      */
-    MBLoadCell(LoadCellModes mode, int serialPortNum, int baudrate);
+    MBLoadCell(USARTInterface &serial, LoadCellModes mode);
 
     /**
      * @brief Initializes the serial communication with the load cell.
@@ -136,7 +135,7 @@ protected:
      *
      * @param buf The message to send.
      */
-    void transmitASCII(std::string buf);
+    void transmitASCII(const std::string &buf);
 
     /**
      * @brief Wrapper to the serial recvString method. This also sets the
@@ -166,7 +165,7 @@ private:
     bool minPrint;
 
     ///< Pointer to the instance of the serial port used for the connection
-    SerialInterface *serial;
+    USARTInterface &serial;
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
index 957f0a04a..c7051b97e 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
@@ -31,9 +31,8 @@ using namespace miosix;
 namespace Boardcore
 {
 
-UBXGPSSerial::UBXGPSSerial(USARTInterface::Baudrate baudrate,
-                           uint8_t sampleRate, USARTType* usartNumber,
-                           USARTInterface::Baudrate defaultBaudrate)
+UBXGPSSerial::UBXGPSSerial(int baudrate, uint8_t sampleRate,
+                           USARTType* usartNumber, int defaultBaudrate)
 {
     this->usart           = nullptr;
     this->baudrate        = baudrate;
@@ -155,13 +154,11 @@ bool UBXGPSSerial::setBaudrateAndUBX(bool safe)
         0x00, 0x00               // reserved2
     };
 
-    int baud = (int)baudrate;
-
     // Prepare baudrate
-    payload[8]  = baud;
-    payload[9]  = baud >> 8;
-    payload[10] = baud >> 16;
-    payload[11] = baud >> 24;
+    payload[8]  = baudrate;
+    payload[9]  = baudrate >> 8;
+    payload[10] = baudrate >> 16;
+    payload[11] = baudrate >> 24;
 
     UBXFrame frame{UBXMessage::UBX_CFG_PRT, payload, sizeof(payload)};
 
@@ -174,7 +171,6 @@ bool UBXGPSSerial::setBaudrateAndUBX(bool safe)
 bool UBXGPSSerial::setSerialCommunication()
 {
     usart = new USART(usartNumber, defaultBaudrate);
-    usart->init();
     // Change the baudrate only if it is different than the default
     if (baudrate != defaultBaudrate)
     {
@@ -303,11 +299,7 @@ bool UBXGPSSerial::writeUBXFrame(const UBXFrame& frame)
     uint8_t packedFrame[frame.getLength()];
     frame.writePacked(packedFrame);
 
-    if (usart->write(packedFrame, frame.getLength()) < 0)
-    {
-        LOG_ERR(logger, "Failed to write ubx message");
-        return false;
-    }
+    usart->write(packedFrame, frame.getLength());
 
     return true;
 }
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.h b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
index 55cba451c..733d2ed6c 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
@@ -63,11 +63,8 @@ public:
      * @param serialPortName Name of the file for the gps device.
      * @param defaultBaudrate Startup baudrate (38400 for NEO-M9N).
      */
-    UBXGPSSerial(
-        USARTInterface::Baudrate baudrate = USARTInterface::Baudrate::B921600,
-        uint8_t sampleRate = 10, USARTType *usartNumber = USART2,
-        USARTInterface::Baudrate defaultBaudrate =
-            USARTInterface::Baudrate::B38400);
+    UBXGPSSerial(int baudrate = 921600, uint8_t sampleRate = 10,
+                 USARTType *usartNumber = USART2, int defaultBaudrate = 38400);
 
     /**
      * @brief Sets up the serial port baudrate, disables the NMEA messages,
@@ -161,8 +158,8 @@ private:
 
     void run() override;
 
-    Boardcore::USARTInterface::Baudrate baudrate;
-    Boardcore::USARTInterface::Baudrate defaultBaudrate;
+    int baudrate;
+    int defaultBaudrate;
     uint8_t sampleRate;  // [Hz]
     USARTType *usartNumber;
     USART *usart;  // The usart interface
diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100.cpp
index 826d51f85..e55f1937f 100644
--- a/src/shared/sensors/VN100/VN100.cpp
+++ b/src/shared/sensors/VN100/VN100.cpp
@@ -27,8 +27,8 @@
 namespace Boardcore
 {
 
-VN100::VN100(USARTType *portNumber, USARTInterface::Baudrate baudRate,
-             CRCOptions crc, uint16_t samplePeriod)
+VN100::VN100(USARTType *portNumber, int baudRate, CRCOptions crc,
+             uint16_t samplePeriod)
     : portNumber(portNumber), baudRate(baudRate), crc(crc)
 {
     this->samplePeriod = samplePeriod;
@@ -151,11 +151,7 @@ bool VN100::sampleRaw()
     }
 
     // Send the IMU sampling command
-    if (!(serialInterface->writeString(preSampleImuString->c_str())))
-    {
-        LOG_WARN(logger, "Unable to sample due to serial communication error");
-        return false;
-    }
+    serialInterface->writeString(preSampleImuString->c_str());
 
     // Wait some time
     // TODO dimension the time
@@ -245,11 +241,7 @@ VN100Data VN100::sampleData()
     }
 
     // Returns Quaternion, Magnetometer, Accelerometer and Gyro
-    if (!(serialInterface->writeString(preSampleImuString->c_str())))
-    {
-        // If something goes wrong i return the last sampled data
-        return lastSample;
-    }
+    serialInterface->writeString(preSampleImuString->c_str());
 
     // Wait some time
     // TODO dimension the time
@@ -277,11 +269,7 @@ VN100Data VN100::sampleData()
     // Returns Magnetometer, Accelerometer, Gyroscope, Temperature and Pressure
     // (UNCOMPENSATED) DO NOT USE THESE MAGNETOMETER, ACCELEROMETER AND
     // GYROSCOPE VALUES
-    if (!(serialInterface->writeString(preSampleTempPressString->c_str())))
-    {
-        // If something goes wrong i return the last sampled data
-        return lastSample;
-    }
+    serialInterface->writeString(preSampleTempPressString->c_str());
 
     // Wait some time
     // TODO dimension the time
@@ -331,10 +319,10 @@ bool VN100::disableAsyncMessages(bool waitResponse)
 bool VN100::configDefaultSerialPort()
 {
     // Initial default settings
-    serialInterface = new USART(portNumber, USARTInterface::Baudrate::B115200);
+    serialInterface = new USART(portNumber, 115200);
 
     // Check correct serial init
-    return serialInterface->init();
+    return true;
 }
 
 /**
@@ -346,7 +334,7 @@ bool VN100::configUserSerialPort()
     std::string command;
 
     // I format the command to change baud rate
-    command = fmt::format("{}{}", "VNWRG,5,", static_cast<int>(baudRate));
+    command = fmt::format("{}{}", "VNWRG,5,", baudRate);
 
     // I can send the command
     if (!sendStringCommand(command))
@@ -361,7 +349,7 @@ bool VN100::configUserSerialPort()
     serialInterface = new USART(portNumber, baudRate);
 
     // Check correct serial init
-    return serialInterface->init();
+    return true;
 }
 
 bool VN100::setCrc(bool waitResponse)
@@ -666,10 +654,7 @@ bool VN100::sendStringCommand(std::string command)
     }
 
     // I send the final command
-    if (!serialInterface->writeString(command.c_str()))
-    {
-        return false;
-    }
+    serialInterface->writeString(command.c_str());
 
     // Wait some time
     // TODO dimension the time
@@ -682,7 +667,7 @@ bool VN100::recvStringCommand(char *command, int maxLength)
 {
     int i = 0;
     // Read the buffer
-    if (!(serialInterface->read(command, maxLength)))
+    if (!(serialInterface->readBlocking(command, maxLength)))
     {
         return false;
     }
diff --git a/src/shared/sensors/VN100/VN100.h b/src/shared/sensors/VN100/VN100.h
index a3f9d63c5..d2d5c6c94 100644
--- a/src/shared/sensors/VN100/VN100.h
+++ b/src/shared/sensors/VN100/VN100.h
@@ -86,10 +86,9 @@ public:
      * @param Redundancy check option.
      * @param samplePeriod Sampling period in ms
      */
-    VN100(USARTType *portNumber    = USART2,
-          USART::Baudrate baudRate = USART::Baudrate::B921600,
-          CRCOptions crc           = CRCOptions::CRC_ENABLE_8,
-          uint16_t samplePeriod    = 20);
+    VN100(USARTType *portNumber = USART2, int baudRate = 921600,
+          CRCOptions crc        = CRCOptions::CRC_ENABLE_8,
+          uint16_t samplePeriod = 20);
 
     bool init() override;
 
@@ -240,7 +239,7 @@ private:
     uint16_t calculateChecksum16(uint8_t *message, int length);
 
     USARTType *portNumber;
-    USART::Baudrate baudRate;
+    int baudRate;
     uint16_t samplePeriod;
     CRCOptions crc;
     bool isInit = false;
diff --git a/src/shared/sensors/VN100/VN100Serial.h b/src/shared/sensors/VN100/VN100Serial.h
deleted file mode 100644
index 22f416772..000000000
--- a/src/shared/sensors/VN100/VN100Serial.h
+++ /dev/null
@@ -1,185 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
- *
- * 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.
- */
-
-/** BRIEF INTRODUCTION
- * This class is used to create a communication easily with UART ports.
- * In particular it handles the creation and the basic primitives
- * for standard communication protocols (init, send, recv and close).
- * The user should pay more attention on the concept behind the use of this
- * class. The principle under the class is that miosix communicates via USART
- * ports using a file based principle (Unix style). Because of that you need to
- * consider that the receive file works as a LIFO queue, in fact the last
- * message is returned over the less recent messages, so if you accumulate
- * various messages of variable length, you might end up with a message which
- * represents the most recent communication + other previous stuff.
- */
-
-#pragma once
-
-#include <arch/common/drivers/serial.h>
-#include <fcntl.h>
-#include <filesystem/file_access.h>
-#include <miosix.h>
-namespace Boardcore
-{
-
-/**
- * @brief Class to communicate with the vn100 via the UART interface.
- */
-class VN100Serial
-{
-public:
-    VN100Serial(unsigned int serialPortNumber, unsigned int serialBaudRate);
-
-    bool init();
-
-    /**
-     * @brief Writes in Serial data.
-     *
-     * Note: to send std::string via serial you need to pass use "c_str()"
-     * method.
-     *
-     * @param data Data to be sent via serial.
-     * @param length Array length.
-     *
-     * @return True if operation succeeded.
-     */
-    template <typename DataSend>
-    bool send(DataSend *data, int length);
-
-    /**
-     * @brief Reads from serial.
-     *
-     * @param data Array where to store data.
-     * @param length Array length.
-     *
-     * @return True if operation succeeded.
-     */
-    template <typename DataReceive>
-    bool recv(DataReceive *data, int length);
-
-    bool closeSerial();
-
-private:
-    unsigned int serialPortNumber;
-    unsigned int serialBaudRate;
-
-    int serialFileDescriptor = 0;
-
-    bool isInit = false;
-};
-
-inline VN100Serial::VN100Serial(unsigned int serialPortNumber,
-                                unsigned int serialBaudRate)
-    : serialPortNumber(serialPortNumber), serialBaudRate(serialBaudRate)
-{
-}
-
-inline bool VN100Serial::init()
-{
-    // If already initialized i avoid the procedure
-    if (isInit)
-    {
-        return true;
-    }
-
-    // Retrieve the file system instance
-    miosix::intrusive_ref_ptr<miosix::DevFs> devFs =
-        miosix::FilesystemManager::instance().getDevFs();
-
-    // Create the serial file which we read and write to communicate
-    if (!(devFs->addDevice(
-            "vn100",
-            miosix::intrusive_ref_ptr<miosix::Device>(
-                new miosix::STM32Serial(serialPortNumber, serialBaudRate)))))
-    {
-        return false;
-    }
-
-    // Open the file
-    serialFileDescriptor = open("/dev/vn100", O_RDWR);
-
-    // Check the descriptor
-    if (serialFileDescriptor <= 0)
-    {
-        return false;
-    }
-
-    // Success
-    isInit = true;
-    return true;
-}
-
-template <typename DataSend>
-inline bool VN100Serial::send(DataSend *data, int length)
-{
-    // Check if the serial has been previously initialized
-    if (!isInit)
-    {
-        return false;
-    }
-
-    // Write the file with the data
-    if (!write(serialFileDescriptor, data, length))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-template <typename DataReceive>
-inline bool VN100Serial::recv(DataReceive *data, int length)
-{
-    if (!isInit)
-    {
-        return false;
-    }
-
-    // Read the data and store it in memory
-    if (!read(serialFileDescriptor, data, length))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-inline bool VN100Serial::closeSerial()
-{
-    // Retrieve the file system instance
-    miosix::intrusive_ref_ptr<miosix::DevFs> devFs =
-        miosix::FilesystemManager::instance().getDevFs();
-
-    // Close the file descriptor
-    close(serialFileDescriptor);
-
-    // Remove the file
-    if (!(devFs->remove("vn100")))
-    {
-        return false;
-    }
-
-    return true;
-}
-
-}  // namespace Boardcore
diff --git a/src/shared/utils/SerialInterface.h b/src/shared/utils/SerialInterface.h
deleted file mode 100644
index 674f7bf3e..000000000
--- a/src/shared/utils/SerialInterface.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Emilio Corigliano
- *
- * 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.
- */
-
-#pragma once
-
-#include <fcntl.h>
-#include <stdio.h>
-#include <utils/Debug.h>
-
-#include <string>
-
-#include "arch/common/drivers/serial.h"
-#include "filesystem/file_access.h"
-#include "miosix.h"
-
-namespace Boardcore
-{
-
-/**
- * @brief Creates and opens a serial port on the board and provides templated
- * "sendData" and "recvData" functions in order to send and receive any data
- * structure needed, or "sendString" and "recvString" in order to send and
- * receive strings.
- */
-class SerialInterface
-{
-public:
-    /**
-     * Constructor of the serial communication with default parameters
-     * @param baudrate port baudrate (default: 19200 as miosix)
-     * @param serialPortNum number from 1 to 3 in order to set the USART to use
-     * (default: 2 [tx_board=PA2  rx_board=PA3 cts=PA0  rts=PA1])
-     * @param serialPortName name of the serial port to open (default:
-     * "simulation")
-     */
-    SerialInterface(int baudrate = 19200, int serialPortNum = 2,
-                    std::string serialPortName = "load_cell")
-    {
-        this->baudrate       = baudrate;
-        this->serialPortNum  = serialPortNum;
-        this->serialPortName = serialPortName;
-        initialized          = false;
-        fd                   = -1;
-    }
-
-    ~SerialInterface()
-    {
-        miosix::intrusive_ref_ptr<miosix::DevFs> devFs =
-            miosix::FilesystemManager::instance().getDevFs();
-        close(fd);
-        devFs->remove(serialPortName.c_str());
-    }
-
-    /**
-     * @brief Initializes the object if it's the first call to this function
-     * @return true if initialization is successful, false otherwise
-     */
-    bool init()
-    {
-        if (initialized)
-        {
-            TRACE(
-                "[SerialCommunication] Error : serial communication already "
-                "initialized!\n");
-            return false;
-        }
-        else if (!serialCommSetup())
-        {
-            TRACE(
-                "[SerialCommunication] Error : can't initialize serial "
-                "communication!\n");
-            return false;
-        }
-
-        initialized = true;
-        return true;
-    }
-
-    /**
-     * @brief Sends a string to the serial
-     * @param data pointer to the string '\0' terminated
-     * @return bytes sent through serial
-     */
-    int sendString(std::string data)
-    {
-        return write(fd, data.c_str(), data.length());
-    }
-
-    /**
-     * @brief Receives a string from the serial
-     * @param data pointer to the destination string
-     * @param maxLen maximum length of the string to read
-     * @return bytes received from serial
-     */
-    int recvString(char *data, int maxLen) { return read(fd, data, maxLen); }
-
-    /**
-     * @brief Receives the data from the simulated sensors and deserializes it
-     * in the templated format
-     * @param data pointer to the struct for the data to receive
-     */
-    template <typename T>
-    void recvData(T *data)
-    {
-        read(fd, data, sizeof(T));
-    }
-
-    /**
-     * @brief Sends the position of the simulated actuators as the templated
-     * type
-     * @param data pointer to the struct of the data to send
-     */
-    template <typename U>
-    void sendData(U *data)
-    {
-        write(fd, data, sizeof(U));
-    }
-
-    std::string getPortName() { return serialPortName; }
-
-    bool isInit() { return initialized; }
-
-private:
-    /* Creates and opens the serial port for communication between OBSW and
-     * simulation device */
-    bool serialCommSetup()
-    {
-        // Takes the file system pointer of the devices
-        miosix::intrusive_ref_ptr<miosix::DevFs> devFs =
-            miosix::FilesystemManager::instance().getDevFs();
-
-        // Creates and adds the serial port to the devices
-        if (!devFs->addDevice(
-                serialPortName.c_str(),
-                miosix::intrusive_ref_ptr<miosix::Device>(
-                    new miosix::STM32Serial(serialPortNum, baudrate))))
-            return false;
-
-        // path string "/dev/<name_of_port>" for the port we want to open
-        std::string serialPortPath = "/dev/" + serialPortName;
-
-        // open serial port
-        fd = open(serialPortPath.c_str(), O_RDWR);
-
-        if (fd <= -1)
-        {
-            TRACE("Cannot open %s\n", serialPortPath.c_str());
-            return false;
-        }
-
-        return true;
-    }
-
-    std::string serialPortName; /**< Port name of the serial port that has to be
-                              created for the communication */
-    int fd; /**< Stores the file descriptor of the serial port file opened for
-               trasmission */
-    int serialPortNum; /**< Stores the USART<serialPortNum> used for the
-                          trasmission range[1:3] */
-    int baudrate;      /**< Baudrate of the serial port */
-    bool initialized;  /**< True if init() already called successfully, false
-                      otherwise */
-};
-
-}  // namespace Boardcore
diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
index 7af7e41c3..12d106ca4 100644
--- a/src/tests/algorithms/NAS/test-nas-parafoil.cpp
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -112,8 +112,7 @@ void init()
     imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
     imu->init();
 
-    gps = new UBXGPSSerial(USARTInterface::Baudrate::B38400, 10, USART2,
-                           USARTInterface::Baudrate::B38400);
+    gps = new UBXGPSSerial(38400, 10, USART2, 38400);
     gps->init();
     gps->start();
 
diff --git a/src/tests/drivers/test-MBLoadCell.cpp b/src/tests/drivers/test-MBLoadCell.cpp
index f5d97bb0d..6b94d3879 100644
--- a/src/tests/drivers/test-MBLoadCell.cpp
+++ b/src/tests/drivers/test-MBLoadCell.cpp
@@ -25,7 +25,8 @@
 #include <string.h>
 #include <utils/ButtonHandler/ButtonHandler.h>
 
-//#define PRINT_ALL_SAMPLES // To be defined if we want to print all the samples
+// #define PRINT_ALL_SAMPLES // To be defined if we want to print all the
+// samples
 
 using namespace Boardcore;
 using namespace miosix;
@@ -58,6 +59,7 @@ int main()
 {
     // In order to disable DEBUG prints of the button press events
     Logging::getStdOutLogSink().setLevel(LOGL_WARNING);
+    USART usart(USART1, 115200);
 
     /**
      * Use of CONT_MOD_TD: transmits net and gross weight
@@ -66,7 +68,7 @@ int main()
      * - use of serial port 2: in stm32f407vg TX=PA2, RX=PA3
      * - use of serial port 1: in stm32f407vg TX=PA9, RX=PA10
      */
-    MBLoadCell loadCell(LoadCellModes::CONT_MOD_T, 2, 115200);
+    MBLoadCell loadCell(usart, LoadCellModes::CONT_MOD_T);
 
     // Instanciating the button
     ButtonHandler::getInstance().registerButtonCallback(
diff --git a/src/tests/drivers/test-mavlink.cpp b/src/tests/drivers/test-mavlink.cpp
index 2f4883c13..8e70f5a51 100644
--- a/src/tests/drivers/test-mavlink.cpp
+++ b/src/tests/drivers/test-mavlink.cpp
@@ -52,7 +52,7 @@ MavDriver* mavlink;
  */
 int main()
 {
-    STM32SerialWrapper serial(USART1, USARTInterface::Baudrate::B19200);
+    STM32SerialWrapper serial(USART1, 19200);
     transceiver = new SerialTransceiver(serial);
     mavlink = new MavDriver(transceiver, nullptr, silenceAfterSend, maxPktAge);
 
diff --git a/src/tests/drivers/test-vn100serial.cpp b/src/tests/drivers/test-vn100serial.cpp
deleted file mode 100644
index 1367ece32..000000000
--- a/src/tests/drivers/test-vn100serial.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
- *
- * 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.
- */
-
-#include <sensors/VN100/VN100Serial.h>
-
-#include <string>
-
-using namespace miosix;
-using namespace std;
-using namespace Boardcore;
-
-int main()
-{
-    char recvString[200];
-    // Object string with the message
-    string message("Communication\n\r");
-
-    GpioPin tx(GPIOB_BASE, 6);
-    GpioPin rx(GPIOB_BASE, 7);
-
-    tx.mode(miosix::Mode::ALTERNATE);
-    rx.mode(miosix::Mode::ALTERNATE);
-
-    tx.alternateFunction(7);
-    rx.alternateFunction(7);
-
-    // Serial inferface
-    VN100Serial serial{1, 115200};
-
-    if (!serial.init())
-    {
-        printf("Init failed\n");
-        return 0;
-    }
-    printf("Success!\n");
-
-    // Loop to send the data via serial
-    while (1)
-    {
-        serial.send(message.c_str(), message.length());
-        // Sleep
-        miosix::Thread::sleep(100);
-        // Receive the string
-        serial.recv(recvString, 200);
-        // Print it out
-        printf("%s\n", recvString);
-    }
-
-    serial.closeSerial();
-
-    return 0;
-}
diff --git a/src/tests/drivers/usart/test-usart.cpp b/src/tests/drivers/usart/test-usart.cpp
index 728f374b3..eb52fa565 100644
--- a/src/tests/drivers/usart/test-usart.cpp
+++ b/src/tests/drivers/usart/test-usart.cpp
@@ -29,7 +29,6 @@
 #include "string"
 #include "string.h"
 #include "thread"
-#include "utils/SerialInterface.h"
 
 using namespace miosix;
 using namespace Boardcore;
@@ -49,6 +48,63 @@ using namespace Boardcore;
  * doesn't have this problem.
  */
 
+// A nice feature of the stm32 is that the USART are connected to the same
+// GPIOS in all families, stm32f1, f2, f4 and l1. Additionally, USART1 and
+// USART6 are always connected to the APB2, while the other USART/UARTs are
+// connected to the APB1.
+
+// USART1: AF7
+typedef miosix::Gpio<GPIOB_BASE, 6> u1tx1;
+typedef miosix::Gpio<GPIOB_BASE, 7> u1rx1;
+typedef miosix::Gpio<GPIOA_BASE, 9> u1tx2;
+typedef miosix::Gpio<GPIOA_BASE, 10> u1rx2;
+// typedef miosix::Gpio<GPIOA_BASE, 11> u1cts;
+// typedef miosix::Gpio<GPIOA_BASE, 12> u1rts;
+
+// USART2: AF7
+typedef miosix::Gpio<GPIOA_BASE, 2> u2tx1;
+typedef miosix::Gpio<GPIOA_BASE, 3> u2rx1;
+typedef miosix::Gpio<GPIOD_BASE, 5> u2tx2;
+typedef miosix::Gpio<GPIOD_BASE, 6> u2rx2;
+// typedef miosix::Gpio<GPIOA_BASE, 0> u2cts;
+// typedef miosix::Gpio<GPIOA_BASE, 1> u2rts;
+
+// USART3: AF7
+typedef miosix::Gpio<GPIOB_BASE, 10> u3tx1;
+typedef miosix::Gpio<GPIOB_BASE, 11> u3rx1;
+typedef miosix::Gpio<GPIOD_BASE, 8> u3tx2;
+typedef miosix::Gpio<GPIOD_BASE, 9> u3rx2;
+// typedef miosix::Gpio<GPIOB_BASE, 13> u3cts;
+// typedef miosix::Gpio<GPIOB_BASE, 14> u3rts;
+
+// UART4: AF8
+typedef miosix::Gpio<GPIOA_BASE, 0> u4tx1;
+typedef miosix::Gpio<GPIOA_BASE, 1> u4rx1;
+typedef miosix::Gpio<GPIOC_BASE, 10> u4tx2;
+typedef miosix::Gpio<GPIOC_BASE, 11> u4rx2;
+
+// UART5: AF8
+typedef miosix::Gpio<GPIOC_BASE, 12> u5tx;
+typedef miosix::Gpio<GPIOD_BASE, 2> u5rx;
+
+// USART6: AF8
+typedef miosix::Gpio<GPIOC_BASE, 6> u6tx1;
+typedef miosix::Gpio<GPIOC_BASE, 7> u6rx1;
+#ifdef STM32F429xx
+typedef miosix::Gpio<GPIOG_BASE, 14> u6tx2;
+typedef miosix::Gpio<GPIOG_BASE, 9> u6rx2;
+
+// USART7: AF8
+typedef miosix::Gpio<GPIOE_BASE, 8> u7tx1;
+typedef miosix::Gpio<GPIOE_BASE, 7> u7rx1;
+typedef miosix::Gpio<GPIOF_BASE, 7> u7tx2;
+typedef miosix::Gpio<GPIOF_BASE, 6> u7rx2;
+
+// USART8: AF8
+typedef miosix::Gpio<GPIOE_BASE, 1> u8tx;
+typedef miosix::Gpio<GPIOE_BASE, 0> u8rx;
+#endif  // STM32F429xx
+
 typedef struct
 {
     char dataChar;
@@ -64,12 +120,8 @@ typedef struct
 } StructToSend;
 StructToSend struct_tx = {'C', 42, 420.69, 48.84};
 char buf_tx[64]        = "Testing communication, but very very very loong :D";
-USARTInterface::Baudrate baudrates[] = {
-    USARTInterface::Baudrate::B2400,   USARTInterface::Baudrate::B9600,
-    USARTInterface::Baudrate::B19200,  USARTInterface::Baudrate::B38400,
-    USARTInterface::Baudrate::B57600,  USARTInterface::Baudrate::B115200,
-    USARTInterface::Baudrate::B230400, USARTInterface::Baudrate::B460800,
-    USARTInterface::Baudrate::B921600};
+int baudrates[]        = {2400,   9600,   19200,  38400,  57600,
+                          115200, 230400, 256000, 460800, 921600};
 
 /**
  * Communication: src -> dst
@@ -77,8 +129,9 @@ USARTInterface::Baudrate baudrates[] = {
  */
 bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst)
 {
-    char buf_rx[64];
-    StructToSend struct_rx;
+    char buf_rx[64] = {0};
+    StructToSend struct_rx{0};
+    size_t nReads{0};
     bool passed = true;
 
     /************************** SENDING STRING **************************/
@@ -87,10 +140,24 @@ bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst)
     printf("\t%d--> sent: \t'%s'\n", src->getId(), buf_tx);
     src->writeString(buf_tx);
     // Thread::sleep(10); // enable to pass the test with STM32SerialWrapper
-    dst->read(buf_rx, 64);
+    if (!dst->readBlocking(buf_rx, 64, nReads))
+    {
+        printf("### NO DATA READ ###\n");
+        passed = false;
+    }
 
     printf("\t%d<-- received: \t'%s'\n", dst->getId(), buf_rx);
 
+    if (nReads != strlen(buf_tx) + 1)
+    {
+        printf("### READ WRONG NUMBER OF BYTES ###\n");
+        passed = false;
+    }
+    else
+    {
+        printf("*** READ EXACT NUMBER OF BYTES ***\n");
+    }
+
     if (strcmp(buf_tx, buf_rx) == 0)
     {
         printf("*** %d -> %d WORKING!\n", src->getId(), dst->getId());
@@ -107,7 +174,12 @@ bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst)
     printf("\t%d--> sent: \t'%s'\n", src->getId(), struct_tx.print().c_str());
     src->write(&struct_tx, sizeof(StructToSend));
     // Thread::sleep(10); // enable to pass the test with STM32SerialWrapper
-    dst->read(&struct_rx, sizeof(StructToSend));
+    if (!dst->readBlocking(&struct_rx, sizeof(StructToSend)))
+    {
+        printf("### NO DATA READ ###\n");
+        passed = false;
+    }
+
     printf("\t%d<-- received: \t'%s'\n", dst->getId(),
            struct_rx.print().c_str());
 
@@ -121,6 +193,13 @@ bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst)
         passed = false;
     }
 
+    // Testing the non blocking read only if USART class
+    if ((dynamic_cast<const USART *>(dst) != nullptr) &&
+        !dynamic_cast<USART *>(dst)->read(&struct_rx, sizeof(StructToSend)))
+    {
+        printf("Non blocking read passed!\n");
+    }
+
     return passed;
 }
 
@@ -137,60 +216,60 @@ bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst)
 int main()
 {
     // Init serial port pins
-    u1rx2::getPin().mode(miosix::Mode::ALTERNATE);
-    u1rx2::getPin().alternateFunction(7);
-    u1tx1::getPin().mode(miosix::Mode::ALTERNATE);
-    u1tx1::getPin().alternateFunction(7);
-
-    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");
-    for (unsigned int iBaud = 0;
-         iBaud < sizeof(baudrates) / sizeof(baudrates[0]); iBaud++)
+    u6rx1::getPin().mode(miosix::Mode::ALTERNATE);
+    u6rx1::getPin().alternateFunction(8);
+    u6tx1::getPin().mode(miosix::Mode::ALTERNATE);
+    u6tx1::getPin().alternateFunction(8);
+
+    u4rx2::getPin().mode(miosix::Mode::ALTERNATE);
+    u4rx2::getPin().alternateFunction(8);
+    u4tx2::getPin().mode(miosix::Mode::ALTERNATE);
+    u4tx2::getPin().alternateFunction(8);
+
+    for (;;)
     {
-        USARTInterface::Baudrate baudrate = baudrates[iBaud];
-        printf("\n\n########################### %d\n", (int)baudrate);
+        bool testPassed = true;
+        printf("*** SERIAL 3 WORKING!\n");
 
-        // declaring the usart peripherals
-        STM32SerialWrapper usartx(USART1, baudrate);
-        usartx.init();
+        for (int baudrate : baudrates)
+        {
+            printf("\n\n########################### %d\n", baudrate);
+            // declaring the usart peripherals
+            USART usartx(USART6, baudrate);
+            // usartx.setBaudrate(baudrate);
+            // usartx.setOversampling(false);
+            // usartx.setStopBits(1);
+            // usartx.setWordLength(USART::WordLength::BIT8);
+            // usartx.setParity(USART::ParityBit::NO_PARITY);
 
-        STM32SerialWrapper usarty(USART2, baudrate);
-        // usarty.setOversampling(false);
-        // usarty.setStopBits(1);
-        // usarty.setWordLength(USART::WordLength::BIT8);
-        // usarty.setParity(USART::ParityBit::NO_PARITY);
-        usarty.init();
+            // USART usarty(UART4, baudrate);
+            STM32SerialWrapper usarty(UART4, baudrate, u4rx2::getPin(),
+                                      u4tx2::getPin());
 
-        // testing transmission (both char and binary) "serial 1 <- serial 2"
-        testPassed &= testCommunicationSequential(&usartx, &usarty);
+            // testing transmission (both char and binary) "serial 1 <- serial
+            // 2"
+            testPassed &= testCommunicationSequential(&usartx, &usarty);
 
-        // testing transmission (both char and binary) "serial 1 -> serial 2"
-        testPassed &= testCommunicationSequential(&usarty, &usartx);
-    }
+            // testing transmission (both char and binary) "serial 1 -> serial
+            // 2"
+            testPassed &= testCommunicationSequential(&usarty, &usartx);
+        }
 
-    if (testPassed)
-    {
-        printf(
-            "********************************\n"
-            "***        TEST PASSED       ***\n"
-            "********************************\n");
-    }
-    else
-    {
-        printf(
-            "################################\n"
-            "###        TEST FAILED       ###\n"
-            "################################\n");
+        if (testPassed)
+        {
+            printf(
+                "********************************\n"
+                "***        TEST PASSED       ***\n"
+                "********************************\n");
+        }
+        else
+        {
+            printf(
+                "################################\n"
+                "###        TEST FAILED       ###\n"
+                "################################\n");
+        }
+        Thread::sleep(5000);
     }
     return 0;
 }
diff --git a/src/tests/radio/test-mavlinkdriver.cpp b/src/tests/radio/test-mavlinkdriver.cpp
index c7d14b510..812c15664 100644
--- a/src/tests/radio/test-mavlinkdriver.cpp
+++ b/src/tests/radio/test-mavlinkdriver.cpp
@@ -36,7 +36,10 @@
 using namespace miosix;
 using namespace Boardcore;
 
-USART usart(USART2, USARTInterface::Baudrate::B115200);
+typedef miosix::Gpio<GPIOA_BASE, 2> u2tx;
+typedef miosix::Gpio<GPIOA_BASE, 3> u2rx;
+
+USART usart(USART2, 115200);
 SerialTransceiver transceiver(usart);
 MavlinkDriver<20, 10> mavlink(&transceiver);
 
@@ -44,10 +47,10 @@ void payloadGenerator();
 
 int main()
 {
-    u2rx1::mode(Mode::ALTERNATE);
-    u2rx1::alternateFunction(7);
-    u2tx1::mode(Mode::ALTERNATE);
-    u2tx1::alternateFunction(7);
+    u2rx::mode(Mode::ALTERNATE);
+    u2rx::alternateFunction(7);
+    u2tx::mode(Mode::ALTERNATE);
+    u2tx::alternateFunction(7);
 
     TaskScheduler scheduler;
     scheduler.addTask(payloadGenerator, 2000);
diff --git a/src/tests/sensors/test-ubxgps-serial.cpp b/src/tests/sensors/test-ubxgps-serial.cpp
index 722cbef73..472f3dcde 100644
--- a/src/tests/sensors/test-ubxgps-serial.cpp
+++ b/src/tests/sensors/test-ubxgps-serial.cpp
@@ -37,8 +37,7 @@ int main()
     printf("Welcome to the ublox test\n");
 
     // Keep GPS baud rate at default for easier testing
-    UBXGPSSerial gps(USARTInterface::Baudrate::B38400, RATE, USART2,
-                     USARTInterface::Baudrate::B9600);
+    UBXGPSSerial gps(38400, RATE, USART2, 9600);
     UBXGPSData dataGPS;
     printf("Gps allocated\n");
 
diff --git a/src/tests/sensors/test-vn100.cpp b/src/tests/sensors/test-vn100.cpp
index 991f26650..2e93b1eff 100644
--- a/src/tests/sensors/test-vn100.cpp
+++ b/src/tests/sensors/test-vn100.cpp
@@ -31,8 +31,7 @@ int main()
 {
     VN100Data sample;
     string sampleRaw;
-    VN100 sensor{USART1, USARTInterface::Baudrate::B921600,
-                 VN100::CRCOptions::CRC_ENABLE_16};
+    VN100 sensor{USART1, 921600, VN100::CRCOptions::CRC_ENABLE_16};
 
     // Let the sensor start up
     Thread::sleep(1000);
diff --git a/src/tests/test-max485.cpp b/src/tests/test-max485.cpp
index 18bb9077b..61a7068b1 100644
--- a/src/tests/test-max485.cpp
+++ b/src/tests/test-max485.cpp
@@ -58,9 +58,9 @@
  *
  */
 
+#include "drivers/usart/USART.h"
 #include "string.h"
 #include "thread"
-#include "utils/SerialInterface.h"
 
 using namespace miosix;
 using namespace Boardcore;
@@ -75,20 +75,16 @@ using ctrlPin2_s2 = miosix::Gpio<GPIOC_BASE, 2>;
 
 char msg[64] = "Testing communication :D";
 char rcv[64];
-int baudrates[] = {2400,   3600,   4800,   9600,  19200,
-                   115200, 230400, 460800, 921600};
+int baudrates[] = {2400,   9600,   19200,  38400,  57600,
+                   115200, 230400, 256000, 460800, 921600};
 
 // function for the thread that has to read from serial
-void readSer(SerialInterface *s)
-{
-    s->recvString(rcv, 64);
-    printf("\t<--%s received: \t'%s'\n", s->getPortName().c_str(), rcv);
-}
+void readSer(USARTInterface &s) {}
 
 // Communicatio: src -> dst
 template <typename GPIO1_src, typename GPIO2_src, typename GPIO1_dst,
           typename GPIO2_dst>
-void testCommunication(char *data, SerialInterface *src, SerialInterface *dst)
+void testCommunication(char *data, USARTInterface &src, USARTInterface &dst)
 {
     // resetting the buffer so precedent tests won't affect this one
     memset(rcv, 0, strlen(rcv) + 1);
@@ -102,21 +98,24 @@ void testCommunication(char *data, SerialInterface *src, SerialInterface *dst)
     GPIO2_dst::low();
 
     // thread that reads from serial
-    std::thread t(readSer, dst);
+    std::thread t(
+        [&]()
+        {
+            dst.readBlocking(rcv, 64);
+            printf("\t<--%d received: \t'%s'\n", dst.getId(), rcv);
+        });
 
-    printf("\t-->%s sending: \t'%s'\n", src->getPortName().c_str(), data);
-    src->sendString(data);
+    printf("\t-->%d sending: \t'%s'\n", src.getId(), data);
+    src.writeString(data);
     t.join();
 
     if (strcmp(data, rcv) == 0)
     {
-        printf("*** %s -> %s WORKING!\n", src->getPortName().c_str(),
-               dst->getPortName().c_str());
+        printf("*** %d -> %d WORKING!\n", src.getId(), dst.getId());
     }
     else
     {
-        printf("### ERROR: %s -> %s!\n", src->getPortName().c_str(),
-               dst->getPortName().c_str());
+        printf("### ERROR: %d -> %d!\n", src.getId(), dst.getId());
     }
 }
 
@@ -136,30 +135,18 @@ int main()
     {
         Thread::sleep(1000);
 
-        // Setting the baudrate to 2400, maximum functioning baudrate for the
-        // Max485 adapters
-        SerialInterface serial1(baudrates[iBaud], 1, "ser1");
-        SerialInterface serial2(baudrates[iBaud], 2, "ser2");
+        // instantiating the two USART drivers
+        USART serial1(USART1, baudrates[iBaud]);
+        USART serial2(USART2, baudrates[iBaud]);
 
-        if (!serial2.init())
-        {
-            printf("[Serial2] Wrong initialization\n");
-            return 1;
-        }
-
-        if (!serial1.init())
-        {
-            printf("[Serial1] Wrong initialization\n");
-            return 1;
-        }
-        printf("\n########################### %d\n", baudrates[iBaud]);
+        printf("\n########################### %d\n", (int)baudrates[iBaud]);
         // testing transmission "serial 1 <- serial 2"
         testCommunication<ctrlPin1_s2, ctrlPin2_s2, ctrlPin1_s1, ctrlPin2_s1>(
-            msg, &serial2, &serial1);
+            msg, serial2, serial1);
 
         // testing transmission "serial 1 -> serial 2"
         testCommunication<ctrlPin1_s1, ctrlPin2_s1, ctrlPin1_s2, ctrlPin2_s2>(
-            msg, &serial1, &serial2);
+            msg, serial1, serial2);
     }
 
     return 0;
-- 
GitLab