✈️ Adds barometer to imu service

esp32/lib/ESP32-sveltekit/IMUService.h

@@ -1,29 +1,59 @@
 #pragma once
 
 #include <MPU6050_light.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BMP085_U.h>
+#include <Adafruit_HMC5883_U.h>
 #include <ArduinoJson.h>
 #include <EventSocket.h>
 
-#define IMU_INTERVAL 200
-#define MAX_ESP_IMU_SIZE 250
+#define IMU_INTERVAL 500
+#define MAX_ESP_IMU_SIZE 500
 #define EVENT_IMU "imu"
 
 class IMUService
 {
 public:
-    IMUService(EventSocket *socket) : _socket(socket), _imu(Wire) {};
+    IMUService(EventSocket *socket) 
+    : 
+#if FT_ENABLED(FT_IMU)
+    _imu(Wire),
+#endif
+#if FT_ENABLED(FT_MAG)
+    _mag(12345),
+#endif
+#if FT_ENABLED(FT_BMP)
+    _bmp(10085),
+#endif
+    _socket(socket)
+    {};
 
     void begin()
     {
+        _socket->registerEvent(EVENT_IMU);
+#if FT_ENABLED(FT_IMU)
         byte status = _imu.begin();
         imu_success = status == 0;
         if(status != 0) {
             ESP_LOGE("IMUService", "MPU initialize failed: %d", status);
         }
-        _socket->registerEvent(EVENT_IMU);
         calibrate();
+#endif
+#if FT_ENABLED(FT_MAG)
+        mag_success = _mag.begin();
+        if(!mag_success) {
+            ESP_LOGE("IMUService", "MAG initialize failed");
+        }
+#endif
+#if FT_ENABLED(FT_BMP)
+        bmp_success = _bmp.begin();
+        if(!bmp_success) {
+            ESP_LOGE("IMUService", "BMP initialize failed");
+        }
+#endif
     };
 
+#if FT_ENABLED(FT_IMU)
     bool isIMUSuccess() {
         return imu_success;
     }
@@ -37,11 +67,11 @@ public:
     }
 
     float getAngleY() {
-        return imu_success ? _imu.getAngleX() : -1;
+        return imu_success ? _imu.getAngleY() : -1;
     }
 
     float getAngleZ() {
-        return imu_success ? _imu.getAngleZ() : -1;
+        return imu_success ? _imu.getGyroZ() : -1;
     }
 
     void calibrate() {
@@ -49,6 +79,45 @@ public:
             _imu.calcOffsets(true, true);
         }
     }
+#endif
+
+#if FT_ENABLED(FT_MAG)
+    float getHeading() {
+        sensors_event_t event; 
+        _mag.getEvent(&event);
+        float heading = atan2(event.magnetic.y, event.magnetic.x);
+        float declinationAngle = 0.22;
+        heading += declinationAngle;
+        if(heading < 0) heading += 2 * PI;
+        if(heading > 2 * PI) heading -= 2 * PI;
+        return heading * 180/M_PI;
+    }
+#endif
+
+#if FT_ENABLED(FT_BMP)
+    bool isBMPSuccess() {
+        return bmp_success;
+    }
+
+    float getAltitude() {
+        sensors_event_t event;
+        _bmp.getEvent(&event);
+        float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+        return bmp_success && event.pressure ? _bmp.pressureToAltitude(seaLevelPressure, event.pressure) : -1;
+    }
+
+    float getPressure() {
+        sensors_event_t event;
+        _bmp.getEvent(&event);
+        return bmp_success && event.pressure ? event.pressure : -1;
+    }
+
+    float getTemperature() {
+        float temperature;
+        _bmp.getTemperature(&temperature);
+        return bmp_success ? temperature : -1;
+    }
+#endif
 
     double round2(double value) {
         return (int)(value * 100 + 0.5) / 100.0;
@@ -69,22 +138,47 @@ protected:
     JsonDocument doc;
 
     void updateImu() {
-        _imu.update();
         doc.clear();
-        doc["x"] = round2(getAngleX());
-        doc["y"] = round2(getAngleY());
-        doc["z"] = round2(getAngleZ());
-        doc["temp"] = round2(getTemp());
-
+#if FT_ENABLED(FT_IMU)
+        if (imu_success){
+            _imu.update();
+            doc["x"] = round2(getAngleX());
+            doc["y"] = round2(getAngleY());
+            doc["z"] = round2(getAngleZ());
+            doc["imu_temp"] = round2(getTemp());
+        }
+#endif
+#if FT_ENABLED(FT_MAG)
+    if (mag_success) {
+        doc["heading"] = round2(getHeading());
+    }
+#endif
+#if FT_ENABLED(FT_BMP)
+    if (bmp_success) {
+        doc["pressure"] = round2(getPressure());
+        doc["altitude"] = round2(getAltitude());
+        doc["bmp_temp"] = round2(getTemperature());
+    }
+#endif
         serializeJson(doc, message);
         _socket->emit(EVENT_IMU, message);
     }
 
 private:
+#if FT_ENABLED(FT_IMU)
     MPU6050 _imu;
+    bool imu_success {false};
+#endif
+#if FT_ENABLED(FT_MAG)
+    Adafruit_HMC5883_Unified _mag;
+    bool mag_success {false};
+#endif
+#if FT_ENABLED(FT_BMP)
+    Adafruit_BMP085_Unified _bmp; 
+    bool bmp_success {false};
+#endif
     EventSocket *_socket;
     unsigned long _lastUpdate {0};
     unsigned long _updateInterval {IMU_INTERVAL};
-    bool imu_success {false};
     char message[MAX_ESP_IMU_SIZE];
 };