Added ICM20948 support

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/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
@@ -35,6 +39,26 @@ class IMU : public SensorBase<IMUAnglesMsg> {
             ESP_LOGE("IMU", "BNO055 initialization failed");
             return false;
         }
+#endif
+#if FT_ENABLED(USE_ICM20948)
+    #if USE_ICM20948_SPIMODE > 0
+        _imu.begin(CS_PIN, SPI_PORT);
+    #else
+        _imu.begin(Wire, 1);
+    #endif
+    if (_imu.status != ICM_20948_Stat_Ok){ 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; }
+    
+    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; }
+    // TODO: Setup low pass filter config
+    _imu.startupMagnetometer();
+    if (_imu.status != ICM_20948_Stat_Ok){ return false; }
         ESP_LOGI("IMU", "BNO055 initialized successfully");
 #endif
         return _msg.success;
@@ -49,7 +73,26 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         _msg.rpy[2] = _imu.getRoll();
         _msg.temperature = _imu.getTemperature();
 #endif
+#if FT_ENABLED(USE_ICM20948)
+        if (_imu.dataReady())
+        {
+            _imu.getAGMT();
+            _msg.rpy[0] = _imu.magX();
+            _msg.rpy[1] = _imu.magY();
+            _msg.rpy[2] = _imu.magZ();
+        }
+#endif
 #if FT_ENABLED(USE_BNO055)
+#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 
+        ICM_20948_I2C _imu;
+    #endif
         if (!_imu.update()) return false;
         _msg.rpy[0] = _imu.getHeading();
         _msg.rpy[1] = _imu.getPitch();
@@ -88,4 +131,14 @@ 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
+        #define SPI_PORT SPI // TODO in periphearals_seetings.h
+        #define CS_PIN 2 
+        ICM_20948_SPI _imu;
+    #else
+        //#define WIRE_PORT Wire 
+        ICM_20948_I2C _imu;
+    #endif
+#endif
 };

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 || USE_ICM20948)
     Magnetometer _mag;
 #endif
 #if FT_ENABLED(USE_BMP180)

esp32/src/features.cpp

@@ -10,6 +10,7 @@ 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");

platformio.ini

@@ -113,6 +113,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