Initialize default DMP for ICM20948

# Conflicts:
#	esp32/include/peripherals/imu.h

app/src/routes/peripherals/i2c/i2c.svelte

@@ -16,6 +16,11 @@
             part_number: 'MPU6050',
             name: 'Six-Axis (Gyro + Accelerometer) MEMS MotionTracking™ Devices'
         },
+        {
+            address: 105,
+            part_number: 'ICM20948',
+            name: 'Nine-Axis (Gyro + Accelerometer + Magnetometer) MEMS MotionTracking™ Device'
+        },
         { address: 115, part_number: 'PAJ7620U2', name: 'Gesture sensor' },
         { address: 119, part_number: 'BMP085', name: 'Temp/Barometric' }
     ]

app/src/routes/peripherals/imu/imu.svelte

@@ -18,10 +18,12 @@
     let angleChartElement: HTMLCanvasElement
     let tempChartElement: HTMLCanvasElement
     let altitudeChartElement: HTMLCanvasElement
+    let magnetometerChartElement: HTMLCanvasElement
 
     let angleChart: Chart
     let tempChart: Chart
     let altitudeChart: Chart
+    let magnetometerChart: Chart
 
     const getChartColors = () => {
         const style = getComputedStyle(document.body)
@@ -171,6 +173,37 @@
                 }
             }
         })
+
+        magnetometerChart = new Chart(magnetometerChartElement, {
+            type: 'line',
+            data: {
+                datasets: [
+                    {
+                        label: 'Heading',
+                        borderColor: colors.primary,
+                        backgroundColor: colors.primary,
+                        borderWidth: 2,
+                        data: $imu.heading,
+                        yAxisID: 'y'
+                    }
+                ]
+            },
+            options: {
+                ...baseConfig,
+                scales: {
+                    ...baseConfig.scales,
+                    y: {
+                        ...baseConfig.scales.y,
+                        title: {
+                            display: true,
+                            text: 'Heading [°]',
+                            color: colors.background,
+                            font: { size: 16, weight: 'bold' }
+                        }
+                    }
+                }
+            }
+        })
     }
 
     const updateChartData = (chart: Chart, data: number[]) => {
@@ -194,6 +227,10 @@
             angleChart.update('none')
         }
 
+        if ($features.mag) {
+            updateChartData(magnetometerChart, $imu.heading)
+        }
+
         if ($features.bmp) {
             updateChartData(tempChart, $imu.bmp_temp)
             updateChartData(altitudeChart, $imu.altitude)
@@ -235,6 +272,17 @@
         </div>
     {/if}
 
+    {#if $features.mag}
+        <div class="w-full overflow-x-auto">
+            <div
+                class="flex w-full flex-col space-y-1 h-60"
+                transition:slide|local={{ duration: 300, easing: cubicOut }}
+            >
+                <canvas bind:this={magnetometerChartElement}></canvas>
+            </div>
+        </div>
+    {/if}
+
     {#if $features.bmp}
         <div class="w-full overflow-x-auto">
             <div

esp32/features.ini

@@ -13,6 +13,8 @@ build_flags =
   -D USE_HMC5883=0
   -D USE_BMP180=0
   -D USE_MPU6050=0
+  -D USE_ICM20948=1
+  -D USE_ICM20948_SPIMODE=1
   -D USE_WS2812=1
   -D USE_BNO055=0
   -D USE_USS=0

esp32/include/features.h

@@ -12,7 +12,7 @@
 #define USE_CAMERA 0
 #endif
 
-// ESP32 IMU on by default
+// ESP32 IMU off by default
 #ifndef USE_MPU6050
 #define USE_MPU6050 0
 #endif
@@ -22,6 +22,14 @@
 #define USE_BNO055 1
 #endif
 
+// ESP32 IMU off by default
+#ifndef USE_ICM20948
+#define USE_ICM20948 0
+#endif
+#ifndef USE_ICM20948_SPIMODE // I2C on by default
+#define USE_ICM20948_SPIMODE 0
+#endif
+
 // ESP32 magnetometer on by default
 #ifndef USE_HMC5883
 #define USE_HMC5883 0

esp32/include/peripherals/barometer.h

@@ -38,7 +38,7 @@ struct BarometerMsg : public SensorMessageBase {
 
 class Barometer : public SensorBase<BarometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _) override {
         _msg.success = _bmp.begin();
         return _msg.success;
     }

esp32/include/peripherals/imu.h

@@ -6,6 +6,10 @@
 #include <ArduinoJson.h>
 #include <utils/math_utils.h>
 
+#if FT_ENABLED(USE_ICM20948)
+#include "ICM_20948.h"
+#endif
+
 #if FT_ENABLED(USE_MPU6050)
 #include <MPU6050_6Axis_MotionApps612.h>
 #endif
@@ -44,7 +48,7 @@ struct IMUAnglesMsg : public SensorMessageBase {
 
 class IMU : public SensorBase<IMUAnglesMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _arg = nullptr) override {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         _msg.success = _imu.testConnection();
@@ -81,12 +85,42 @@ class IMU : public SensorBase<IMUAnglesMsg> {
             return false;
         }
         _imu.setExtCrystalUse(true);
+#endif
+#if FT_ENABLED(USE_ICM20948)
+    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+        #define ICM_20948_USE_DMP // TODO: Move to features.ini
+        SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1); // TODO: Move to global spi start
+        _imu = (ICM_20948_SPI*)_arg;
+        #ifndef ICM20948_ALIVE
+        #define ICM20948_ALIVE
+        _imu->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode");
+        _imu->initializeDMP();
+        #endif
+    #else
+        _imu = (ICM_20948_I2C*)_arg;
+        #ifndef ICM20948_ALIVE
+        #define ICM20948_ALIVE
+        _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode");
+        #endif
+        
+    #endif
+    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){ 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){ ESP_LOGW("IMU", "Failed to start ICM20948: set full scale failed"); return false; }
+    // TODO: Setup low pass filter config
+    _msg.success = true;
 #endif
         return _msg.success;
     }
 
     bool update() override {
-        if (!_msg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
         uint16_t fifoCount = _imu.getFIFOCount();
         uint8_t intStatus = _imu.getIntStatus();
@@ -105,6 +139,21 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         }
         return false;
 #endif
+#if FT_ENABLED(USE_ICM20948)
+          
+        #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
+        #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
+        if (_imu->dataReady())
+        {
+            _imu->getAGMT();
+        } else {
+            return false;
+        }
+        #endif
+        _msg.rpy[0] = _imu->accX();
+        _msg.rpy[1] = _imu->accY();
+        _msg.rpy[2] = _imu->accZ();
+#endif
 #if FT_ENABLED(USE_BNO055)
         sensors_event_t event;
         _imu.getEvent(&event);
@@ -134,4 +183,12 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_BNO055)
     Adafruit_BNO055 _imu {55, 0x29};
 #endif
+#if FT_ENABLED(USE_ICM20948)
+    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+        ICM_20948_SPI* _imu;
+    #else
+        //#define WIRE_PORT Wire 
+        ICM_20948_I2C* _imu;
+    #endif
+#endif
 };

esp32/include/peripherals/magnetometer.h

@@ -11,6 +11,7 @@
 
 #include <peripherals/sensor.hpp>
 
+
 struct MagnetometerMsg : public SensorMessageBase {
     float rpy[3] {0, 0, 0};
     float heading {-1};
@@ -38,20 +39,59 @@ struct MagnetometerMsg : public SensorMessageBase {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize() override {
-        _msg.success = _mag.begin();
+    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;
+                #ifndef ICM20948_ALIVE
+                #define ICM20948_ALIVE
+                _imu->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode");
+                #endif
+            #else
+                _mag = (ICM_20948_I2C*)_arg;
+                #ifndef ICM20948_ALIVE
+                #define ICM20948_ALIVE
+                if (!_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
+                #endif
+                
+            #endif
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            
+            _mag->startupMagnetometer();
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            _msg.success = true;
+        #elif FT_ENABLED(USE_HMC5883)
+            _msg.success = _mag.begin();
+        #endif
         return _msg.success;
     }
 
     bool update() override {
         if (!_msg.success) return false;
-        sensors_event_t event;
-        bool updated = _mag.getEvent(&event);
-        if (!updated) return false;
-        _msg.rpy[0] = event.magnetic.x;
-        _msg.rpy[1] = event.magnetic.y;
-        _msg.rpy[2] = event.magnetic.z;
-        _msg.heading = atan2(event.magnetic.y, event.magnetic.x);
+        #if FT_ENABLED(USE_ICM20948)
+        #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
+        #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
+        if (_imu->dataReady())
+        { 
+            _imu->getAGMT();
+        } else {
+            return false;
+        }
+        #endif
+        _msg.rpy[0] = _mag->magX();
+        _msg.rpy[1] = _mag->magY();
+        _msg.rpy[2] = _mag->magZ();
+
+        #elif FT_ENABLED(USE_HMC5883)
+            sensors_event_t event;
+            bool updated = _mag.getEvent(&event);
+            if (!updated) return false;
+            _msg.rpy[0] = event.magnetic.x;
+            _msg.rpy[1] = event.magnetic.y;
+            _msg.rpy[2] = event.magnetic.z;
+        #endif
+        _msg.heading = atan2(_msg.rpy[1], _msg.rpy[0]); // atan2(y, x)
         _msg.heading += declinationAngle;
         if (_msg.heading < 0) _msg.heading += 2 * PI;
         if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
@@ -68,6 +108,18 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
     float getHeading() { return _msg.heading; }
 
   private:
-    Adafruit_HMC5883_Unified _mag {12345};
+    
+    #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* _mag;
+        #else
+            //#define WIRE_PORT Wire 
+            ICM_20948_I2C* _mag;
+        #endif
+    #elif FT_ENABLED(USE_HMC5883)
+        Adafruit_HMC5883_Unified _mag {12345};
+    #endif
     const float declinationAngle = 0.22;
 };

esp32/include/peripherals/peripherals.h

@@ -87,10 +87,10 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     JsonDocument doc;
     char message[MAX_ESP_IMU_SIZE];
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     IMU _imu;
 #endif
-#if FT_ENABLED(USE_HMC5883)
+#if FT_ENABLED(USE_HMC5883) || FT_ENABLED(USE_ICM20948)
     Magnetometer _mag;
 #endif
 #if FT_ENABLED(USE_BMP180)

esp32/include/peripherals/sensor.hpp

@@ -17,7 +17,7 @@ class SensorBase {
   public:
     SensorBase() {}
 
-    virtual bool initialize() = 0;
+    virtual bool initialize(void* _arg) = 0;
 
     virtual bool update() = 0;
 

esp32/include/settings/peripherals_settings.h

@@ -15,7 +15,7 @@
 #define SCL_PIN SCL
 #endif
 #ifndef I2C_FREQUENCY
-#define I2C_FREQUENCY 1000000UL
+#define I2C_FREQUENCY 400000UL
 #endif
 
 class PinConfig {

esp32/include/utils/timing.h

@@ -39,3 +39,49 @@
         name##_count = 0;                                                    \
         last_time = esp_timer_get_time() / 1000;                             \
     }
+
+#define CALLS_PER_SECOND_TIMED_START_TICK(name, function)                        \
+    static uint64_t name##_##function##_start = 0;                           \
+    static uint64_t name##_##function##_total_time = 0;                      \
+    static uint64_t name##_##function##_call_count = 0;                      \
+    name##_##function##_start = esp_timer_get_time();
+
+#define CALLS_PER_SECOND_TIMED_END_TICK(name, function)                          \
+    name##_##function##_total_time += (esp_timer_get_time() - name##_##function##_start); \
+    name##_##function##_call_count++;
+
+#define CALLS_PER_SECOND_TIMED_CALL(name, function, call)                        \
+    static uint64_t name##_##function##_total_time = 0;                          \
+    static uint64_t name##_##function##_call_count = 0;                          \
+    do {                                                                         \
+        uint64_t name##_##function##_start = esp_timer_get_time();               \
+        call;                                                                    \
+        name##_##function##_total_time += (esp_timer_get_time() - name##_##function##_start); \
+        name##_##function##_call_count++;                                        \
+    } while (0)
+
+#define CALLS_PER_SECOND_TIMED_FUNC_PRINT(name, function)                        \
+    if (name##_##function##_call_count > 0) {                                    \
+        uint64_t avg = name##_##function##_total_time / name##_##function##_call_count; \
+        if (avg < 1000) {                                                        \
+            ESP_LOGI("Timing", "  %s: %llu us (avg over %llu calls)",            \
+                     #function, avg, name##_##function##_call_count);            \
+        } else {                                                                 \
+            ESP_LOGI("Timing", "  %s: %llu ms (avg over %llu calls)",            \
+                     #function, avg / 1000, name##_##function##_call_count);     \
+        }                                                                        \
+        name##_##function##_total_time = 0;                                      \
+        name##_##function##_call_count = 0;                                      \
+    }
+
+#define CALLS_PER_SECOND_TIMED(name, ...)                                        \
+    do {                                                                         \
+        static uint64_t name##_last_print = 0;                                   \
+        uint64_t name##_current_time = esp_timer_get_time() / 1000;              \
+        if (name##_current_time - name##_last_print >= 1000) {                   \
+            ESP_LOGI("Timing", "=== %s Average Timings ===", #name);             \
+            __VA_ARGS__                                                          \
+            name##_last_print = name##_current_time;                             \
+        }                                                                        \
+    } while (0)
+

esp32/src/features.cpp

@@ -12,11 +12,14 @@ void printFeatureConfiguration() {
     ESP_LOGI("Features", "USE_MOTION: %s", USE_MOTION ? "enabled" : "disabled");
 
     // Sensors
+    ESP_LOGI("Features", "USE_ICM20948: %s", USE_ICM20948 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_BNO055: %s", USE_BNO055 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_MPU6050: %s", USE_MPU6050 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_HMC5883: %s", USE_HMC5883 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_BMP180: %s", USE_BMP180 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_USS: %s", USE_USS ? "enabled" : "disabled");
+    ESP_LOGI("Features", "USE_PAJ7620U2: %s", USE_PAJ7620U2 ? "enabled" : "disabled");
+    
 
     // Peripherals
     ESP_LOGI("Features", "USE_PCA9685: %s", USE_PCA9685 ? "enabled" : "disabled");
@@ -31,8 +34,8 @@ void printFeatureConfiguration() {
 
 void features(JsonObject &root) {
     root["camera"] = USE_CAMERA ? true : false;
-    root["imu"] = (USE_MPU6050 || USE_BNO055) ? true : false;
-    root["mag"] = (USE_HMC5883 || USE_BNO055) ? true : false;
+    root["imu"] = (USE_MPU6050 || USE_BNO055 || USE_ICM20948) ? true : false;
+    root["mag"] = (USE_HMC5883 || USE_BNO055 || USE_ICM20948) ? true : false;
     root["bmp"] = USE_BMP180 ? true : false;
     root["sonar"] = USE_USS ? true : false;
     root["servo"] = USE_PCA9685 ? true : false;

esp32/src/main.cpp

@@ -124,15 +124,20 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
     motionService.begin();
 
     for (;;) {
-        CALLS_PER_SECOND(SpotControlLoopEntry);
-        peripherals.update();
-        motionService.update(&peripherals);
-        servoController.setAngles(motionService.getAngles());
-        servoController.update();
+        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, peripherals_update, peripherals.update());
+        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, motionService_update, motionService.update(&peripherals));
+        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, servoController_setAngles, servoController.setAngles(motionService.getAngles()));
+        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, servoController_update, servoController.update());
 #if FT_ENABLED(USE_WS2812)
         ledService.loop();
 #endif
-        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        // CALLS_PER_SECOND_TIMED(SpotControlLoopEntry,
+        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, peripherals_update)
+        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, motionService_update)
+        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, servoController_setAngles)
+        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, servoController_update)
+        // );
+        // vTaskDelayUntil(&xLastWakeTime, xFrequency);
     }
 }
 

esp32/src/peripherals/peripherals.cpp

@@ -15,18 +15,40 @@ void Peripherals::begin() {
 
     updatePins();
 
+#if FT_ENABLED(USE_ICM20948)
+    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+        ICM_20948_SPI* icm20948 = new ICM_20948_SPI;
+    #else
+        ICM_20948_I2C* icm20948 = new ICM_20948_I2C;
+    #endif
+#endif
+
+// --- IMU ---
 #if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-    if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
+    if (!_imu.initialize(nullptr)) ESP_LOGE("Peripherals", "IMU initialize failed");
+#elif FT_ENABLED(USE_ICM20948)
+    if (!_imu.initialize(icm20948)) ESP_LOGE("Peripherals", "IMU initialize failed (ICM20948)");
 #endif
+
+// --- MAGNETOMETER ---
 #if FT_ENABLED(USE_HMC5883)
-    if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
+    if (!_mag.initialize(nullptr)) ESP_LOGE("Peripherals", "MAG initialize failed");
+#elif FT_ENABLED(USE_ICM20948)
+    if (!_mag.initialize(icm20948)) ESP_LOGE("Peripherals", "MAG initialize failed (ICM20948)");
 #endif
+
+// --- BMP ---
 #if FT_ENABLED(USE_BMP180)
-    if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
+    if (!_bmp.initialize(nullptr)) ESP_LOGE("Peripherals", "BMP initialize failed");
 #endif
+
+
+// --- GESTURE ---
 #if FT_ENABLED(USE_PAJ7620U2)
-    if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed");
+    if (!_gesture.initialize(nullptr)) ESP_LOGE("Peripherals", "Gesture initialize failed");
 #endif
+
+// --- SONAR ---
 #if FT_ENABLED(USE_USS)
     _left_sonar = std::make_unique<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
     _right_sonar = std::make_unique<NewPing>(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
@@ -34,11 +56,25 @@ void Peripherals::begin() {
 };
 
 void Peripherals::update() {
-    readImu();
-    readMag();
-    EXECUTE_EVERY_N_MS(100, { readGesture(); });
-    EXECUTE_EVERY_N_MS(500, { readBMP(); });
-    EXECUTE_EVERY_N_MS(500, { readSonar(); });
+    bool res = true;
+    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_imu, res = readImu());
+    #ifdef FT_ENABLED(USE_ICM20948)
+        // IF ICM_20948 fails to get IMU, it means that mag also does not have new data
+        CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, if (res) { res = readMag(); } );
+    #else
+        CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, res = readMag());
+    #endif
+    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_gesture, EXECUTE_EVERY_N_MS(100, { readGesture(); }) );
+    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_bmp, EXECUTE_EVERY_N_MS(500, { readBMP(); }) );
+    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_sonar, EXECUTE_EVERY_N_MS(500, { readSonar(); }) );
+
+    CALLS_PER_SECOND_TIMED(Peripherals_update,
+        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_imu)
+        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_mag)
+        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_gesture)
+        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_bmp)
+        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_sonar)
+    );
 }
 
 void Peripherals::updatePins() {
@@ -48,6 +84,7 @@ void Peripherals::updatePins() {
 
     if (state().sda != -1 && state().scl != -1) {
         Wire.begin(state().sda, state().scl, state().frequency);
+        ESP_LOGI("Peripherals", "Starting Wire with SDA=%d, SCL=%d, FREQ=%d", state().sda, state().scl, state().frequency);
         i2c_active = true;
     }
 }
@@ -79,7 +116,7 @@ void Peripherals::scanI2C(uint8_t lower, uint8_t higher) {
 /* IMU FUNCTIONS */
 bool Peripherals::readImu() {
     bool updated = false;
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     beginTransaction();
     updated = _imu.update();
     endTransaction();
@@ -89,7 +126,7 @@ bool Peripherals::readImu() {
 
 bool Peripherals::readMag() {
     bool updated = false;
-#if FT_ENABLED(USE_HMC5883)
+#if FT_ENABLED(USE_HMC5883 || USE_ICM20948)
     beginTransaction();
     updated = _mag.update();
     endTransaction();
@@ -127,7 +164,7 @@ void Peripherals::readSonar() {
 
 float Peripherals::angleX() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
         _imu.getAngleX();
 #else
         0;
@@ -136,7 +173,7 @@ float Peripherals::angleX() {
 
 float Peripherals::angleY() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
         _imu.getAngleY();
 #else
         0;
@@ -145,7 +182,7 @@ float Peripherals::angleY() {
 
 float Peripherals::angleZ() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
         _imu.getAngleZ();
 #else
         0;
@@ -165,11 +202,11 @@ float Peripherals::leftDistance() { return _left_distance; }
 float Peripherals::rightDistance() { return _right_distance; }
 
 void Peripherals::getIMUResult(JsonVariant &root) {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     JsonVariant imu = root["imu"].to<JsonVariant>();
     _imu.getResults(imu);
 #endif
-#if FT_ENABLED(USE_HMC5883)
+#if FT_ENABLED(USE_HMC5883 || USE_ICM20948) // TODO:
     JsonVariant mag = root["mag"].to<JsonVariant>();
     _mag.getResults(mag);
 #endif

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
@@ -116,6 +121,7 @@ lib_deps =
 	adafruit/Adafruit Unified Sensor@^1.1.14
     adafruit/Adafruit BNO055@^1.6.4
 	FastLED@3.5.0
+    sparkfun/SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library@^1.3.2
 extra_scripts = 
 	pre:esp32/scripts/pre_build.py
     pre:esp32/scripts/build_app.py