diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
index bb69eafc4a487c9bdb142b8c524f27fec0a3e630..c17d3417f1d76117d0f070baaa74db1056576d0a 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
@@ -48,7 +48,7 @@ bool LIS2MDL::init()
     {
         // Important! It is imperative to get the 4WSPI enabled (set to the
         // value of 1) due to the four-wire connection for SPI and the I2C_DIS
-        // must be disabled. In addiction, self test is enabled.
+        // must be disabled.
         SPITransaction spi(mSlave);
         spi.writeRegister(CFG_REG_C, (1 << 2) | (1 << 5));
     }
@@ -94,6 +94,7 @@ bool LIS2MDL::selfTest()
     float avgX = 0.f, avgY = 0.f, avgZ = 0.f;
 
     {
+        // selfTest is enabled
         SPITransaction spi(mSlave);
         uint16_t temp = spi.readRegister(CFG_REG_C) | (1 << 1);
         spi.writeRegister(CFG_REG_C, temp);
@@ -131,6 +132,7 @@ bool LIS2MDL::selfTest()
             passed = false;
 
     {
+        // Disable selfTest
         SPITransaction spi(mSlave);
         uint16_t temp = spi.readRegister(CFG_REG_C) & ~(1 << 1);
         spi.writeRegister(CFG_REG_C, temp);
@@ -153,7 +155,7 @@ bool LIS2MDL::applyConfig(Config config)
     SPITransaction spi(mSlave);
     uint8_t reg = 0;
 
-    // CFG_REG_A
+    // CFG_REG_A: configuration register
     reg |= config.odr << 2;
     reg |= config.deviceMode;
     reg |= (1 << 7);
diff --git a/src/tests/sensors/test-lis2mdl.cpp b/src/tests/sensors/test-lis2mdl.cpp
index d0032fa7f3b55efc7b4273aa20dcfc0239b5b733..87353379647449e94a024ea0cd51926e78a25afd 100644
--- a/src/tests/sensors/test-lis2mdl.cpp
+++ b/src/tests/sensors/test-lis2mdl.cpp
@@ -21,6 +21,7 @@
  */
 
 #include <drivers/spi/SPIDriver.h>
+#include <drivers/usart/USART.h>
 #include <miosix.h>
 #include <sensors/LIS2MDL/LIS2MDL.h>
 #include <utils/Debug.h>
@@ -31,7 +32,7 @@ using namespace miosix;
 int main()
 {
     GpioPin cs(GPIOA_BASE, 15), miso(GPIOA_BASE, 6), mosi(GPIOA_BASE, 7),
-        clk(GPIOA_BASE, 5);
+        clk(GPIOA_BASE, 5), tx(GPIOA_BASE, 2), rx(GPIOA_BASE, 3);
 
     cs.mode(Mode::OUTPUT);
     cs.high();
@@ -46,16 +47,24 @@ int main()
     mosi.mode(Mode::ALTERNATE);
     mosi.alternateFunction(5);
 
+    // Serial data write on Matlab
+    rx.mode(Mode::ALTERNATE);
+    rx.alternateFunction(7);
+
+    tx.mode(Mode::ALTERNATE);
+    tx.alternateFunction(7);
+
     SPIBus bus(SPI1);
+    USART usart(USART2, USARTInterface::Baudrate::B115200);
+    usart.init();
 
     SPIBusConfig busConfig;
     busConfig.clockDivider = SPI::ClockDivider::DIV_256;
     busConfig.mode         = SPI::Mode::MODE_3;
 
     LIS2MDL::Config config;
-    config.odr        = LIS2MDL::ODR_10_HZ;
-    config.deviceMode = LIS2MDL::MD_CONTINUOUS;
-    // config.scale              = LIS2MDL::FS_16_GAUSS;
+    config.odr                = LIS2MDL::ODR_10_HZ;
+    config.deviceMode         = LIS2MDL::MD_CONTINUOUS;
     config.temperatureDivider = 5;
 
     LIS2MDL sensor(bus, cs, busConfig, config);
@@ -74,7 +83,8 @@ int main()
     }
     TRACE("selfTest returned true\n");
     TRACE("Now printing some sensor data:\n");
-    Thread::sleep(100);
+    Thread::sleep(3000);
+    float sensorData[3];
 
     while (true)
     {
@@ -82,6 +92,10 @@ int main()
         LIS2MDLData data = sensor.getLastSample();
         TRACE("%f C | x: %f | y: %f | z: %f\n", data.temperature,
               data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
-        miosix::Thread::sleep(100);
+        sensorData[0] = data.magneticFieldX;
+        sensorData[1] = data.magneticFieldY;
+        sensorData[2] = data.magneticFieldZ;
+        usart.write(sensorData, 3 * sizeof(float));
+        miosix::Thread::sleep(10);
     }
 }