From f78a0f50bda84557b0cee26a0a6a91df00faa7c2 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 27 Nov 2025 20:51:54 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20imu=20and=20magnotometer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/peripherals/imu.h | 49 +++++++++++++++++------- esp32/include/peripherals/magnetometer.h | 35 ++++++++--------- esp32/src/main.cpp | 12 +++--- 3 files changed, 58 insertions(+), 38 deletions(-) diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h index 7ea4c1d..d4e34b8 100644 --- a/esp32/include/peripherals/imu.h +++ b/esp32/include/peripherals/imu.h @@ -48,39 +48,60 @@ class IMU : public SensorBase { #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); #endif - return true; + return _msg.success; } 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 @@ -105,7 +126,7 @@ class IMU : public SensorBase { private: #if FT_ENABLED(USE_MPU6050) MPU6050 _imu; - uint8_t devStatus {false}; + uint8_t devStatus {0}; Quaternion q; uint8_t fifoBuffer[64]; VectorFloat gravity; diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h index 5877347..c9ca221 100644 --- a/esp32/include/peripherals/magnetometer.h +++ b/esp32/include/peripherals/magnetometer.h @@ -38,37 +38,36 @@ struct MagnetometerMsg : public SensorMessageBase { class Magnetometer : public SensorBase { 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; }; \ No newline at end of file diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index 5c7d1b8..4d7ce83 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -132,12 +132,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(); - peripherals.getIMUResult(results); - socket.emit(EVENT_IMU, results); - }); vTaskDelayUntil(&xLastWakeTime, xFrequency); } } @@ -164,6 +158,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(); + peripherals.getIMUResult(results); + socket.emit(EVENT_IMU, results); + }); vTaskDelay(100 / portTICK_PERIOD_MS); }