Added ICM20948 support

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)