diff --git a/.vscode/settings.json b/.vscode/settings.json
index d9a4771aeed495aca7823138f665f4b73954d2fd..b90bff8b4f7f467c18dcf057d5f34cc3f21c17a5 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -302,9 +302,11 @@
                 "velnord",
                 "vout",
                 "vsense",
+                "WSPI",
                 "Xbee",
                 "xnord",
-                "yned"
+                "yned",
+                "Zyxda"
         ],
         "cSpell.language": "en",
         "cSpell.enabled": true
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
index 7d92b0163c3ea53bd9c465239dca77c009cfd09c..15bab01737118e118b49204001271d61fa112858 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
@@ -46,9 +46,9 @@ bool LIS2MDL::init()
     }
 
     {
-        // The 4WSPI enabled, I2C_DIS is disabled and selfTest is disabled.
+        // selfTest is disabled
         SPITransaction spi(mSlave);
-        spi.writeRegister(CFG_REG_C, (1 << 2) | (1 << 5));
+        spi.writeRegister(CFG_REG_C, ENABLE_4WSPI | I2C_DISABLE);
     }
 
     {
@@ -95,12 +95,11 @@ bool LIS2MDL::selfTest()
     // Set configuration for selfTest procedure. selfTest still not enabled
     {
         SPITransaction spi(mSlave);
-        // Continuous mode, odr = 100 Hz, enable temperature compensation
-        spi.writeRegister(CFG_REG_A, 140);
-        // Offset cancellation
-        spi.writeRegister(CFG_REG_B, spi.readRegister(CFG_REG_B) | (1 << 1));
-        // BDU enabled
-        spi.writeRegister(CFG_REG_C, spi.readRegister(CFG_REG_C) | (1 << 4));
+        spi.writeRegister(CFG_REG_A,
+                          ENABLE_TEMPERATURE_COMP | ODR_100_HZ | MD_CONTINUOUS);
+        spi.writeRegister(CFG_REG_B,
+                          spi.readRegister(CFG_REG_B) | OFFSET_CANCELLATION);
+        spi.writeRegister(CFG_REG_C, spi.readRegister(CFG_REG_C) | ENABLE_BDU);
     }
     miosix::Thread::sleep(20);
 
@@ -121,7 +120,8 @@ bool LIS2MDL::selfTest()
     {
         // selfTest is enabled
         SPITransaction spi(mSlave);
-        spi.writeRegister(CFG_REG_C, spi.readRegister(CFG_REG_C) | (1 << 1));
+        spi.writeRegister(CFG_REG_C,
+                          spi.readRegister(CFG_REG_C) | ENABLE_SELF_TEST);
     }
     miosix::Thread::sleep(60);
 
@@ -150,11 +150,8 @@ bool LIS2MDL::selfTest()
     {
         // Disable selfTest
         SPITransaction spi(mSlave);
-        spi.writeRegister(CFG_REG_C, spi.readRegister(CFG_REG_C) & ~(1 << 1));
-        // Set idle mode
-        uint16_t reg = spi.readRegister(CFG_REG_A);
-        reg |= 0b11;
-        spi.writeRegister(CFG_REG_A, reg);
+        spi.writeRegister(CFG_REG_C,
+                          spi.readRegister(CFG_REG_C) & ~ENABLE_SELF_TEST);
     }
 
     return true;
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.h b/src/shared/sensors/LIS2MDL/LIS2MDL.h
index c65c07b976b73a4fdc14f58556c071736e364823..0bde9df3e88878a92d68dae5cac2b5e49bc1493c 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.h
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.h
@@ -157,13 +157,12 @@ private:
     static constexpr float LSB_PER_GAUSS_MIN     = 0.001395;
     static constexpr float LSB_PER_GAUSS_MAX     = 0.001605;
 
-    static constexpr uint32_t ENABLE_TEMPERATURE = 0x80;
-    static constexpr uint32_t ENABLE_SELF_TEST   = 0x02;
-    static constexpr uint32_t ENABLE_BDU         = 0x10;
-    static constexpr uint32_t ENABLE_INT_PIN     = 0x01;
-    static constexpr uint32_t ENABLE_INT_X       = 0x80;
-    static constexpr uint32_t ENABLE_INT_Y       = 0x40;
-    static constexpr uint32_t ENABLE_INT_Z       = 0x20;
+    static constexpr uint32_t ENABLE_TEMPERATURE_COMP = (1 << 7);
+    static constexpr uint32_t ENABLE_SELF_TEST        = (1 << 1);
+    static constexpr uint32_t ENABLE_BDU              = (1 << 4);
+    static constexpr uint32_t ENABLE_4WSPI            = (1 << 2);
+    static constexpr uint32_t OFFSET_CANCELLATION     = (1 << 1);
+    static constexpr uint32_t I2C_DISABLE             = (1 << 5);
 
     PrintLogger logger = Logging::getLogger("lis2mdl");
 };