From 72214183782629c3b74a34bc17f0d0f3b45529c7 Mon Sep 17 00:00:00 2001
From: Niklas Jensen <niklaspj2004@gmail.com>
Date: Fri, 26 Dec 2025 13:02:35 +0100
Subject: [PATCH] Enable SPI for icm20948

---
 esp32/features.ini                       |  2 +-
 esp32/include/peripherals/imu.h          | 15 +++++++--------
 esp32/include/peripherals/magnetometer.h |  2 +-
 esp32/src/peripherals/peripherals.cpp    |  2 +-
 4 files changed, 10 insertions(+), 11 deletions(-)

diff --git a/esp32/features.ini b/esp32/features.ini
index 2f8189a..12aec1c 100644
--- a/esp32/features.ini
+++ b/esp32/features.ini
@@ -14,7 +14,7 @@ build_flags =
   -D USE_BMP180=0
   -D USE_MPU6050=0
   -D USE_ICM20948=1
-  -D USE_ICM20948_SPIMODE=0
+  -D USE_ICM20948_SPIMODE=1
   -D USE_WS2812=1
   -D USE_BNO055=0
   -D USE_USS=0
diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index 2036e38..5a60791 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -41,7 +41,10 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         }
 #endif
 #if FT_ENABLED(USE_ICM20948)
-    #if USE_ICM20948_SPIMODE > 0
+    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+        #define SPI_PORT SPI
+        #define CS_PIN 2 
+        SPI_PORT.begin(41U, 19U, 20U, -1);
         _imu = (ICM_20948_SPI*)_arg;
         if (true || !_imu->isConnected()) { _imu->begin(CS_PIN, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode"); }
     #else
@@ -49,16 +52,16 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         if (true || !_imu->isConnected()) { _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode"); }
         
     #endif
-    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
+    if (_imu->status != ICM_20948_Stat_Ok){ ESP_LOGW("IMU", "Failed to start ICM20948: begin failed"); return false; }
     
     _imu->setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
-    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
+    if (_imu->status != ICM_20948_Stat_Ok){ ESP_LOGW("IMU", "Failed to start ICM20948: set sample failed"); return false; }
     
     ICM_20948_fss_t myFSS;
     myFSS.a = gpm2;
     myFSS.g = dps250;
     _imu->setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
-    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
+    if (_imu->status != ICM_20948_Stat_Ok){ ESP_LOGW("IMU", "Failed to start ICM20948: set full scale failed"); return false; }
     // TODO: Setup low pass filter config
     _msg.success = true;
 #endif
@@ -87,8 +90,6 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #endif
 #if FT_ENABLED(USE_ICM20948)
     #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-        #define SPI_PORT SPI // TODO in periphearals_seetings.h
-        #define CS_PIN 2 
         ICM_20948_SPI _imu;
     #else
         //#define WIRE_PORT Wire 
@@ -134,8 +135,6 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #endif
 #if FT_ENABLED(USE_ICM20948)
     #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-        #define SPI_PORT SPI // TODO in periphearals_seetings.h
-        #define CS_PIN 2 
         ICM_20948_SPI* _imu;
     #else
         //#define WIRE_PORT Wire 
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index 9e8fbac..f81881a 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -14,7 +14,7 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
     bool initialize(void* _arg) override {
         #if FT_ENABLED(USE_ICM20948)
-            #if USE_ICM20948_SPIMODE > 0
+            #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
                 _mag = (ICM_20948_SPI*)_arg;
                 if (true || !_mag->isConnected()) { _mag->begin(CS_PIN, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode"); }
             #else
diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp
index 8dc53ff..5d5353f 100644
--- a/esp32/src/peripherals/peripherals.cpp
+++ b/esp32/src/peripherals/peripherals.cpp
@@ -17,7 +17,7 @@ void Peripherals::begin() {
     updatePins();
 
 #if FT_ENABLED(USE_ICM20948)
-    #if USE_ICM20948_SPIMODE > 0
+    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
         ICM_20948_SPI* icm20948 = new ICM_20948_SPI;
     #else
         ICM_20948_I2C* icm20948 = new ICM_20948_I2C;