From 59bb1d9579d827c53ecf9e1453c5896c447cec05 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Mon, 8 Sep 2025 22:37:57 +0200
Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improves=20imu=20speed=20b?=
 =?UTF-8?q?y=20making=20it=20non=20blocking=20and=20run=20faster?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/include/peripherals/imu.h | 30 ++++++++++++++++++++----------
 1 file changed, 20 insertions(+), 10 deletions(-)

diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index 4de5fe5..05c4ec3 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -50,12 +50,18 @@ class IMU {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         imuMsg.success = _imu.testConnection();
-        devStatus = _imu.dmpInitialize();
         if (!imuMsg.success) return false;
-        _imu.setDMPEnabled(true);
-        _imu.setI2CMasterModeEnabled(false);
-        _imu.setI2CBypassEnabled(true);
-        _imu.setSleepEnabled(false);
+        devStatus = _imu.dmpInitialize();
+        if (devStatus == 0) {
+            _imu.setDMPEnabled(false);
+            _imu.setDMPConfig1(0x03);
+            _imu.setDMPEnabled(true);
+            _imu.setI2CMasterModeEnabled(false);
+            _imu.setI2CBypassEnabled(true);
+            _imu.setSleepEnabled(false);
+        } else {
+            return false;
+        }
 #endif
 #if FT_ENABLED(USE_BNO055)
         imuMsg.success = _imu.begin();
@@ -70,11 +76,15 @@ class IMU {
     bool readIMU() {
         if (!imuMsg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
-        bool updated = _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
-        _imu.dmpGetQuaternion(&q, fifoBuffer);
-        _imu.dmpGetGravity(&gravity, &q);
-        _imu.dmpGetYawPitchRoll(imuMsg.rpy, &q, &gravity);
-        return updated;
+        if (_imu.dmpPacketAvailable()) {
+            if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
+                _imu.dmpGetQuaternion(&q, fifoBuffer);
+                _imu.dmpGetGravity(&gravity, &q);
+                _imu.dmpGetYawPitchRoll(imuMsg.rpy, &q, &gravity);
+                return true;
+            }
+        }
+        return false;
 #endif
 #if FT_ENABLED(USE_BNO055)
         sensors_event_t event;