diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index 5a60791..a2cf3c0 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -5,7 +5,7 @@
 #include <peripherals/sensor.hpp>
 
 #if FT_ENABLED(USE_ICM20948)
-#include "ICM_20948.h" 
+#include "ICM_20948.h"
 #endif
 
 #if FT_ENABLED(USE_MPU6050)
@@ -42,14 +42,12 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #endif
 #if FT_ENABLED(USE_ICM20948)
     #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-        #define SPI_PORT SPI
-        #define CS_PIN 2 
-        SPI_PORT.begin(41U, 19U, 20U, -1);
+        SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1);
         _imu = (ICM_20948_SPI*)_arg;
-        if (true || !_imu->isConnected()) { _imu->begin(CS_PIN, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode"); }
+        if (!_imu->isConnected()) { _imu->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode"); }
     #else
         _imu = (ICM_20948_I2C*)_arg;
-        if (true || !_imu->isConnected()) { _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode"); }
+        if (!_imu->isConnected()) { _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode"); }
         
     #endif
     if (_imu->status != ICM_20948_Stat_Ok){ ESP_LOGW("IMU", "Failed to start ICM20948: begin failed"); return false; }
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index f81881a..f255e34 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -15,11 +15,12 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
     bool initialize(void* _arg) override {
         #if FT_ENABLED(USE_ICM20948)
             #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+                SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1);
                 _mag = (ICM_20948_SPI*)_arg;
-                if (true || !_mag->isConnected()) { _mag->begin(CS_PIN, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode"); }
+                if (!_mag->isConnected()) { _mag->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode"); }
             #else
                 _mag = (ICM_20948_I2C*)_arg;
-                if (true || !_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
+                if (!_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
                 
             #endif
             if (_mag->status != ICM_20948_Stat_Ok){ return false; }
diff --git a/platformio.ini b/platformio.ini
index 3846904..b62c793 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -55,6 +55,10 @@ build_flags =
 	-D USS_RIGHT_PIN=14
 	-D SDA_PIN=47
 	-D SCL_PIN=21
+    -D SPI_SCK=41
+    -D SPI_MISO=19
+    -D SPI_MOSI=20
+    -D ICM20948_SPI_CS=2 # Only needed if ICM20948 is in SPI mode
 
 [env:seeed-xiao-esp32s3]
 platform = espressif32
@@ -90,6 +94,7 @@ build_flags =
 	${factory_settings.build_flags}
     ${features.build_flags}
 	${build_settings.build_flags}
+    -D SPI_PORT=SPI # Define which spi port to use for external components
 	-D CORE_DEBUG_LEVEL=4
 	-D register=
 	-std=gnu++2a