Early stages of magnetometer from ICM20948

esp32/include/peripherals/imu.h

@@ -43,10 +43,11 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_ICM20948)
     #if USE_ICM20948_SPIMODE > 0
         _imu = (ICM_20948_SPI*)_arg;
-        _imu->begin(CS_PIN, SPI_PORT);
+        if (true || !_imu->isConnected()) { _imu->begin(CS_PIN, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode"); }
     #else
         _imu = (ICM_20948_I2C*)_arg;
-        _imu->begin(Wire, 1, 0xFF);
+        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; }
     
@@ -59,8 +60,6 @@ class IMU : public SensorBase<IMUAnglesMsg> {
     _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; }
 #endif
         return _msg.success;
     }

esp32/include/peripherals/magnetometer.h

@@ -12,23 +12,48 @@ struct MagnetometerMsg {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize(void* _) override {
-        _msg.success = _mag.begin();
-        if (_msg.success) {
-            ESP_LOGI("MAG", "HMC5883L initialized successfully");
-        } else {
-            ESP_LOGE("MAG", "HMC5883L initialization failed");
-        }
+    bool initialize(void* _arg) override {
+        #if FT_ENABLED(USE_ICM20948)
+            #if 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
+                _mag = (ICM_20948_I2C*)_arg;
+                if (true || !_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
+                
+            #endif
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            
+            _mag->startupMagnetometer();
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+        #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;
-        _msg.rpy[0] = _mag.getMagX();
-        _msg.rpy[1] = _mag.getMagY();
-        _msg.rpy[2] = _mag.getMagZ();
-        _msg.heading = _mag.getHeading();
+        #if FT_ENABLED(USE_ICM20948)
+            _mag->getAGMT();
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            _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 +63,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

@@ -77,7 +77,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     IMU _imu;
 #endif
-#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
+#if FT_ENABLED(USE_HMC5883) || FT_ENABLED(USE_ICM20948)
     Magnetometer _mag;
 #endif
 #if FT_ENABLED(USE_BMP180)