🐛 Fix imu and magnotometer

esp32/include/peripherals/imu.h

@@ -52,22 +52,36 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         _msg.success = _imu.testConnection();
-        if (!_msg.success) return false;
+        if (!_msg.success) {
+            ESP_LOGE("IMU", "MPU6050 connection test failed");
+            return false;
+        }
         devStatus = _imu.dmpInitialize();
         if (devStatus == 0) {
-            _imu.setDMPEnabled(false);
-            _imu.setDMPConfig1(0x03);
-            _imu.setDMPEnabled(true);
+            _imu.setXGyroOffset(0);
+            _imu.setYGyroOffset(0);
+            _imu.setZGyroOffset(0);
+            _imu.setXAccelOffset(0);
+            _imu.setYAccelOffset(0);
+            _imu.setZAccelOffset(0);
+
             _imu.setI2CMasterModeEnabled(false);
             _imu.setI2CBypassEnabled(true);
             _imu.setSleepEnabled(false);
+            _imu.setRate(1);
+            _imu.resetFIFO();
+            _imu.setDMPEnabled(true);
+
+            ESP_LOGI("IMU", "MPU6050 DMP initialized successfully");
         } else {
-            return false;
+            ESP_LOGE("IMU", "DMP initialization failed (code %d)", devStatus);
+            _msg.success = false;
         }
 #endif
 #if FT_ENABLED(USE_BNO055)
         _msg.success = _imu.begin();
         if (!_msg.success) {
+            ESP_LOGE("IMU", "BNO055 connection test failed");
             return false;
         }
         _imu.setExtCrystalUse(true);
@@ -98,13 +112,20 @@ class IMU : public SensorBase<IMUAnglesMsg> {
     bool update() override {
         //if (!_msg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
-        if (_imu.dmpPacketAvailable()) {
-            if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
-                _imu.dmpGetQuaternion(&q, fifoBuffer);
-                _imu.dmpGetGravity(&gravity, &q);
-                _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
-                return true;
-            }
+        uint16_t fifoCount = _imu.getFIFOCount();
+        uint8_t intStatus = _imu.getIntStatus();
+
+        if (intStatus & 0x10) {
+            _imu.resetFIFO();
+            ESP_LOGW("IMU", "FIFO overflow, resetting");
+            return false;
+        }
+
+        if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
+            _imu.dmpGetQuaternion(&q, fifoBuffer);
+            _imu.dmpGetGravity(&gravity, &q);
+            _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
+            return true;
         }
         return false;
 #endif
@@ -148,7 +169,7 @@ class IMU : public SensorBase<IMUAnglesMsg> {
   private:
 #if FT_ENABLED(USE_MPU6050)
     MPU6050 _imu;
-    uint8_t devStatus {false};
+    uint8_t devStatus {0};
     Quaternion q;
     uint8_t fifoBuffer[64];
     VectorFloat gravity;

esp32/include/peripherals/magnetometer.h

@@ -38,37 +38,36 @@ struct MagnetometerMsg : public SensorMessageBase {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize() {
-        msg.success = _mag.begin();
-        return msg.success;
+    bool initialize() override {
+        _msg.success = _mag.begin();
+        return _msg.success;
     }
 
-    bool update() {
-        if (!msg.success) return false;
+    bool update() override {
+        if (!_msg.success) return false;
         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;
-        msg.heading = atan2(event.magnetic.y, event.magnetic.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;
+        _msg.rpy[0] = event.magnetic.x;
+        _msg.rpy[1] = event.magnetic.y;
+        _msg.rpy[2] = event.magnetic.z;
+        _msg.heading = atan2(event.magnetic.y, event.magnetic.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;
     }
 
-    float getMagX() { return msg.rpy[0]; }
+    float getMagX() { return _msg.rpy[0]; }
 
-    float getMagY() { return msg.rpy[1]; }
+    float getMagY() { return _msg.rpy[1]; }
 
-    float getMagZ() { return msg.rpy[2]; }
+    float getMagZ() { return _msg.rpy[2]; }
 
-    float getHeading() { return msg.heading; }
+    float getHeading() { return _msg.heading; }
 
   private:
     Adafruit_HMC5883_Unified _mag {12345};
-    MagnetometerMsg msg;
     const float declinationAngle = 0.22;
 };

esp32/src/main.cpp

@@ -185,12 +185,6 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
 #if FT_ENABLED(USE_WS2812)
         ledService.loop();
 #endif
-        EXECUTE_EVERY_N_MS(250, {
-            JsonDocument doc;
-            JsonVariant results = doc.to<JsonVariant>();
-            peripherals.getIMUResult(results);
-            socket.emit(EVENT_IMU, results);
-        });
         vTaskDelayUntil(&xLastWakeTime, xFrequency);
     }
 }
@@ -216,6 +210,12 @@ void IRAM_ATTR serviceLoopEntry(void *) {
         wifiService.loop();
         apService.loop();
         EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics(socket));
+        EXECUTE_EVERY_N_MS(500, {
+            JsonDocument doc;
+            JsonVariant results = doc.to<JsonVariant>();
+            peripherals.getIMUResult(results);
+            socket.emit(EVENT_IMU, results);
+        });
 
         vTaskDelay(100 / portTICK_PERIOD_MS);
     }