🪮 Moves imu definition to own service

esp32/lib/ESP32-sveltekit/Peripherals.h

@@ -15,19 +15,19 @@
 #include <SPI.h>
 #include <Wire.h>
 
-#include <MPU6050_6Axis_MotionApps612.h>
 #include <Adafruit_PWMServoDriver.h>
 #include <Adafruit_BMP085_U.h>
 #include <Adafruit_HMC5883_U.h>
 #include <Adafruit_Sensor.h>
 #include <Adafruit_ADS1X15.h>
 #include <NewPing.h>
+#include <peripherals/imu.h>
 
 #define EVENT_CONFIGURATION_SETTINGS "peripheralSettings"
 
 #define EVENT_I2C_SCAN "i2cScan"
 
-#define I2C_INTERVAL 5000
+#define I2C_INTERVAL 250
 #define MAX_ESP_IMU_SIZE 500
 #define EVENT_IMU "imu"
 #define EVENT_SERVO_STATE "servoState"
@@ -100,16 +100,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #endif
 
 #if FT_ENABLED(USE_IMU)
-        _imu.initialize();
+        if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
-        imu_success = _imu.testConnection();
-        devStatus = _imu.dmpInitialize();
-        if (!imu_success) {
-            ESP_LOGE("IMUService", "MPU initialize failed");
-        }
-        _imu.setDMPEnabled(true);
-        _imu.setI2CMasterModeEnabled(false);
-        _imu.setI2CBypassEnabled(true);
-        _imu.setSleepEnabled(false);
 #endif
 #if FT_ENABLED(USE_MAG)
         mag_success = _mag.begin();
@@ -140,7 +131,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     void loop() {
         EXECUTE_EVERY_N_MS(_updateInterval, {
             beginTransaction();
-            updateImu();
+            emitIMU();
             readSonar();
             emitSonar();
             endTransaction();
@@ -254,46 +245,13 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     bool readIMU() {
         bool updated = false;
 #if FT_ENABLED(USE_IMU)
-        updated = imu_success && _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
+        beginTransaction();
-        _imu.dmpGetQuaternion(&q, fifoBuffer);
+        updated = _imu.readIMU();
-        _imu.dmpGetGravity(&gravity, &q);
+        endTransaction();
-        _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 #endif
         return updated;
     }
 
-    float getTemp() {
-        float temp = -1;
-#if FT_ENABLED(USE_IMU)
-        temp = imu_success ? imu_temperature : -1;
-#endif
-        return temp;
-    }
-
-    float getAngleX() {
-        float angle = 0;
-#if FT_ENABLED(USE_IMU)
-        angle = imu_success ? ypr[0] * 180 / M_PI : 0;
-#endif
-        return angle;
-    }
-
-    float getAngleY() {
-        float angle = 0;
-#if FT_ENABLED(USE_IMU)
-        angle = imu_success ? ypr[1] * 180 / M_PI : 0;
-#endif
-        return angle;
-    }
-
-    float getAngleZ() {
-        float angle = 0;
-#if FT_ENABLED(USE_IMU)
-        angle = imu_success ? ypr[2] * 180 / M_PI : 0;
-#endif
-        return angle;
-    }
-
     /* MAG FUNCTIONS */
     float getHeading() {
         float heading = 0;
@@ -341,40 +299,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         return temperature;
     }
 
-    StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
-
-  protected:
-    void updateImu() {
-        doc.clear();
-        bool newData = false;
-#if FT_ENABLED(USE_IMU)
-        newData = imu_success && readIMU();
-        if (imu_success) {
-            doc["x"] = round2(getAngleX());
-            doc["y"] = round2(getAngleY());
-            doc["z"] = round2(getAngleZ());
-        }
-#endif
-#if FT_ENABLED(USE_MAG)
-        newData = newData || mag_success;
-        if (mag_success) {
-            doc["heading"] = round2(getHeading());
-        }
-#endif
-#if FT_ENABLED(USE_BMP)
-        newData = newData || bmp_success;
-        if (bmp_success) {
-            doc["pressure"] = round2(getPressure());
-            doc["altitude"] = round2(getAltitude());
-            doc["bmp_temp"] = round2(getTemperature());
-        }
-#endif
-        if (newData) {
-            serializeJson(doc, message);
-            socket.emit(EVENT_IMU, message);
-        }
-    }
-
     void readSonar() {
 #if FT_ENABLED(USE_USS)
         _left_distance = _left_sonar->ping_cm();
@@ -383,6 +307,33 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #endif
     }
 
+    float leftDistance() { return _left_distance; }
+    float rightDistance() { return _right_distance; }
+
+    StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
+
+    void emitIMU() {
+        doc.clear();
+        JsonObject root = doc.to<JsonObject>();
+#if FT_ENABLED(USE_IMU)
+        _imu.readIMU(root);
+#endif
+#if FT_ENABLED(USE_MAG)
+        if (mag_success) {
+            doc["heading"] = round2(getHeading());
+        }
+#endif
+#if FT_ENABLED(USE_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);
+    }
+
     void emitSonar() {
 #if FT_ENABLED(USE_USS)
 
@@ -392,9 +343,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #endif
     }
 
-    float leftDistance() { return _left_distance; }
-    float rightDistance() { return _right_distance; }
-
   private:
     EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
     FSPersistence<PeripheralsConfiguration> _persistence;
@@ -414,14 +362,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     uint16_t target_pwm[16] = {0};
 #endif
 #if FT_ENABLED(USE_IMU)
-    MPU6050 _imu;
+    IMU _imu;
-    bool imu_success {false};
-    uint8_t devStatus {false};
-    Quaternion q;
-    uint8_t fifoBuffer[64];
-    VectorFloat gravity;
-    float ypr[3];
-    float imu_temperature {-1};
 #endif
 #if FT_ENABLED(USE_MAG)
     Adafruit_HMC5883_Unified _mag;

esp32/lib/ESP32-sveltekit/peripherals/imu.h

@@ -0,0 +1,66 @@
+#ifndef IMU_h
+#define IMU_h
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <MathUtils.h>
+
+#include <MPU6050_6Axis_MotionApps612.h>
+
+class IMU {
+  public:
+    IMU() {}
+    bool initialize() {
+        _imu.initialize();
+        imu_success = _imu.testConnection();
+        devStatus = _imu.dmpInitialize();
+        if (!imu_success) return false;
+        _imu.setDMPEnabled(true);
+        _imu.setI2CMasterModeEnabled(false);
+        _imu.setI2CBypassEnabled(true);
+        _imu.setSleepEnabled(false);
+        return true;
+    }
+
+    bool readIMU() {
+        if (!imu_success) return false;
+        bool updated = _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
+        _imu.dmpGetQuaternion(&q, fifoBuffer);
+        _imu.dmpGetGravity(&gravity, &q);
+        _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        return updated;
+    }
+
+    float getTemperature() { return imu_success ? imu_temperature : -1; }
+
+    float getAngleX() { return imu_success ? ypr[0] * 180 / M_PI : 0; }
+
+    float getAngleY() { return imu_success ? ypr[1] * 180 / M_PI : 0; }
+
+    float getAngleZ() { return imu_success ? ypr[2] * 180 / M_PI : 0; }
+
+    Quaternion* getQuaternion() { return &q; }
+
+    void readIMU(JsonObject& root) {
+        if (!imu_success) return;
+        root["x"] = round2(getAngleX());
+        root["y"] = round2(getAngleY());
+        root["z"] = round2(getAngleZ());
+    }
+
+    bool active() { return imu_success; }
+
+  private:
+    MPU6050 _imu;
+    bool imu_success {false};
+    uint8_t devStatus {false};
+    Quaternion q;
+    uint8_t fifoBuffer[64];
+    VectorFloat gravity;
+    float ypr[3];
+    float imu_temperature {-1};
+};
+
+#endif