add icm to imt feature list

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

@@ -245,6 +245,10 @@
             angleChart.update('none')
         }
 
+        if ($features.mag) {
+            updateChartData(magnetometerChart, $imu.heading)
+        }
+
         if ($features.bmp && tempChart && altitudeChart) {
             updateChartData(
                 tempChart,

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=0
   -D USE_WS2812=1
   -D USE_BNO055=0
   -D USE_USS=0

esp32/include/features.h

@@ -19,6 +19,15 @@
 #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
 #endif

esp32/include/peripherals/barometer.h

@@ -13,7 +13,7 @@ struct BarometerMsg {
 
 class Barometer : public SensorBase<BarometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _) {
         _msg.success = _bmp.begin();
         if (_msg.success) {
             ESP_LOGI("BMP", "BMP180 initialized successfully");

esp32/include/peripherals/imu.h

@@ -4,6 +4,10 @@
 #include <features.h>
 #include <peripherals/sensor.hpp>
 
+#if FT_ENABLED(USE_ICM20948)
+#include "ICM_20948.h"
+#endif
+
 #if FT_ENABLED(USE_MPU6050)
 #include <peripherals/drivers/mpu6050.h>
 #endif
@@ -20,7 +24,7 @@ struct IMUAnglesMsg {
 
 class IMU : public SensorBase<IMUAnglesMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _arg = nullptr) override {
 #if FT_ENABLED(USE_MPU6050)
         _msg.success = _imu.begin();
         if (!_msg.success) {
@@ -35,13 +39,43 @@ class IMU : public SensorBase<IMUAnglesMsg> {
             ESP_LOGE("IMU", "BNO055 initialization failed");
             return false;
         }
-        ESP_LOGI("IMU", "BNO055 initialized successfully");
+#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 (!_msg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
         if (!_imu.update()) return false;
         _msg.rpy[0] = _imu.getYaw();
@@ -49,11 +83,22 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         _msg.rpy[2] = _imu.getRoll();
         _msg.temperature = _imu.getTemperature();
 #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)
-        if (!_imu.update()) return false;
-        _msg.rpy[0] = _imu.getHeading();
-        _msg.rpy[1] = _imu.getPitch();
-        _msg.rpy[2] = _imu.getRoll();
 #endif
         return true;
     }
@@ -88,4 +133,12 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_BNO055)
     BNO055Driver _imu;
 #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

@@ -12,23 +12,63 @@ struct MagnetometerMsg {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _arg) override {
-        _msg.success = _mag.begin();
+        #if FT_ENABLED(USE_ICM20948)
-        if (_msg.success) {
+            #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-            ESP_LOGI("MAG", "HMC5883L initialized successfully");
+                SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1);
-        } else {
+                _mag = (ICM_20948_SPI*)_arg;
-            ESP_LOGE("MAG", "HMC5883L initialization failed");
+                #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;
-        if (!_mag.update()) return false;
+        #if FT_ENABLED(USE_ICM20948)
-        _msg.rpy[0] = _mag.getMagX();
+        #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
-        _msg.rpy[1] = _mag.getMagY();
+        #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
-        _msg.rpy[2] = _mag.getMagZ();
+        if (_imu->dataReady())
-        _msg.heading = _mag.getHeading();
+        { 
+            _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;
+        _msg.heading *= 180 / M_PI;
         return true;
     }
 
@@ -38,5 +78,18 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
     float getHeading() { return _msg.heading; }
 
   private:
-    HMC5883LDriver _mag;
+    
+    #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

@@ -74,10 +74,10 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     inline void endTransaction() { xSemaphoreGiveRecursive(_accessMutex); }
 
-#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

@@ -3,7 +3,7 @@
 template <class T>
 class SensorBase {
   public:
-    virtual bool initialize() = 0;
+    virtual bool initialize(void* _arg) = 0;
     virtual bool update() = 0;
     virtual bool isActive() { return _msg.success; }
 

esp32/include/settings/peripherals_settings.h

@@ -13,7 +13,7 @@
 #define SCL_PIN SCL
 #endif
 #ifndef I2C_FREQUENCY
-#define I2C_FREQUENCY 1000000UL
+#define I2C_FREQUENCY 400000UL
 #endif
 
 // Use proto types directly

esp32/include/utils/timing.h

@@ -40,6 +40,51 @@
         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)
+
 #define WARN_IF_SLOW(name, period_ms)                                                                    \
     static uint64_t name##_slow_count = 0;                                                               \
     static uint64_t name##_slow_last_time = 0;                                                           \

esp32/src/features.cpp

@@ -10,11 +10,14 @@ void printFeatureConfiguration() {
     ESP_LOGI("Features", "USE_CAMERA: %s", USE_CAMERA ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_MOTION: %s", USE_MOTION ? "enabled" : "disabled");
 
+    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");
+    
 
     ESP_LOGI("Features", "USE_PCA9685: %s", USE_PCA9685 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_WS2812: %s", USE_WS2812 ? "enabled" : "disabled");
@@ -27,8 +30,8 @@ void printFeatureConfiguration() {
 
 void features_request(const socket_message_FeaturesDataRequest& fd_req, socket_message_FeaturesDataResponse& fd_res) {
     fd_res.camera = USE_CAMERA ? true : false;
-    fd_res.imu = (USE_MPU6050 || USE_BNO055) ? true : false;
+    fd_res.imu = (USE_MPU6050 || USE_BNO055 || USE_ICM20948) ? true : false;
-    fd_res.mag = (USE_HMC5883 || USE_BNO055) ? true : false;
+    fd_res.mag = (USE_HMC5883 || USE_BNO055 || USE_ICM20948) ? true : false;
     fd_res.bmp = USE_BMP180 ? true : false;
     fd_res.sonar = USE_USS ? true : false;
     fd_res.servo = USE_PCA9685 ? true : false;

esp32/src/main.cpp

@@ -264,7 +264,13 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
 #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

@@ -16,18 +16,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);
@@ -74,7 +96,7 @@ void Peripherals::scanI2C(uint8_t lower, uint8_t higher) {
 }
 
 void Peripherals::getIMUProto(socket_message_IMUData &data) {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     data.x = _imu.getAngleX();
     data.y = _imu.getAngleY();
     data.z = _imu.getAngleZ();
@@ -101,7 +123,7 @@ void Peripherals::getSettingsProto(socket_message_PeripheralSettingsData &data)
 /* 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();
@@ -111,7 +133,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();
@@ -149,7 +171,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;
@@ -158,7 +180,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;
@@ -167,7 +189,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;
@@ -187,7 +209,7 @@ float Peripherals::leftDistance() { return _left_distance; }
 float Peripherals::rightDistance() { return _right_distance; }
 
 bool Peripherals::calibrateIMU() {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     beginTransaction();
     bool result = _imu.calibrate();
     endTransaction();

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
@@ -113,6 +118,7 @@ lib_deps =
 	ArduinoJson@>=7.0.0
 	teckel12/NewPing@^1.9.7
 	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