From c8cd5cd8df46c618b2f44a659730dd14ea3c0320 Mon Sep 17 00:00:00 2001
From: NepEgor <enepochatyy@gmail.com>
Date: Sat, 14 May 2022 22:19:56 +0600
Subject: [PATCH] removed gyro int; fixed with ps/2 update

---
 include/input_mapper.h |  5 ++++-
 src/gyro.cpp           | 15 ++++++---------
 src/input_mapper.cpp   | 22 +++++++++++++---------
 src/main.cpp           | 22 +++++++++++++---------
 4 files changed, 36 insertions(+), 28 deletions(-)

diff --git a/include/input_mapper.h b/include/input_mapper.h
index 015874a..49cbb79 100644
--- a/include/input_mapper.h
+++ b/include/input_mapper.h
@@ -9,7 +9,10 @@ namespace InputMapper
 
     void mapTrackpad(uint8_t id, uint8_t fid, int32_t x, int32_t y, int32_t dx, int32_t dy, uint32_t time);
 
-    void update(uint32_t time, bool &gyro_ready);
+    bool gyroEnabled();
+    void gyroUpdate();
+
+    void update(uint32_t time);
 
     void mapTriggers(uint32_t value[2]);
 
diff --git a/src/gyro.cpp b/src/gyro.cpp
index f65046a..02509d6 100644
--- a/src/gyro.cpp
+++ b/src/gyro.cpp
@@ -53,9 +53,9 @@ void Gyro::init()
     y_filter.init(filter_size);
     z_filter.init(filter_size);
 
-    mpu.setIntDataReadyEnabled(1);
+    //mpu.setIntDataReadyEnabled(1);
 
-    mpu.setDLPFMode(MPU6050_DLPF_BW_5);
+    //mpu.setDLPFMode(MPU6050_DLPF_BW_5);
 
     _Enabled = [] { return false; };
 }
@@ -67,12 +67,9 @@ void Gyro::setEnabledCallback(bool (*Enabled)())
 
 void Gyro::update()
 {
-    if (Enabled())
-    {
-        mpu.getRotation(&x, &y, &z);
+    mpu.getRotation(&x, &y, &z);
 
-        x = x_filter.filter(x);
-        y = y_filter.filter(y);
-        z = z_filter.filter(z);
-    }
+    x = x_filter.filter(x);
+    y = y_filter.filter(y);
+    z = z_filter.filter(z);
 }
diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp
index 201c549..1cc965b 100644
--- a/src/input_mapper.cpp
+++ b/src/input_mapper.cpp
@@ -175,8 +175,8 @@ namespace InputMapper
         }
 
         gyro.init();
-        //gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; });
-        gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; });
+        gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; });
+        //gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; });
 
         device.begin();
     }
@@ -254,7 +254,17 @@ namespace InputMapper
         }
     }
 
-    void update(uint32_t time, bool &gyro_ready)
+    bool gyroEnabled()
+    {
+        return gyro.Enabled();
+    }
+
+    void gyroUpdate()
+    {
+        gyro.update();
+    }
+
+    void update(uint32_t time)
     {
         for (uint8_t id = 0; id < 2; ++id)
         {
@@ -276,12 +286,6 @@ namespace InputMapper
 
             }
         }
-
-        if (gyro_ready)
-        {
-            gyro.update();
-            gyro_ready = false;
-        }
     }
 
     void mapTriggers(uint32_t value[2])
diff --git a/src/main.cpp b/src/main.cpp
index 91da62d..175e255 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -31,11 +31,6 @@ TrackPad trackpad[2]; // 0 - left, 1 - right
 
 int32_t trackpad_maxX, trackpad_maxY;
 
-bool mpuInterrupt = false;
-void dmpDataReady() {
-    mpuInterrupt = true;
-}
-
 void setup()
 {
     // Turn on LED
@@ -63,8 +58,6 @@ void setup()
     trackpad_maxX = trackpad[0].getMaxX();
     trackpad_maxY = trackpad[0].getMaxY();
 
-    attachInterrupt(gyro_int, dmpDataReady, RISING);
-
     InputMapper::begin();
 
     // Turn off LED
@@ -123,7 +116,7 @@ void loop()
             }
         }
     }
-    
+
     uint32_t triggers[] = {analogRead(pin_trigger[0]), analogRead(pin_trigger[1])};
     InputMapper::mapTriggers(triggers);
 
@@ -139,7 +132,18 @@ void loop()
         }
     }
 
-    InputMapper::update(micros(), mpuInterrupt);
+    static uint32_t gyro_start = micros();
+    if (InputMapper::gyroEnabled())
+    {
+        uint32_t gyro_now = micros();
+        if (gyro_now - gyro_start > 1000)
+        {
+            gyro_start = gyro_now;
+            InputMapper::gyroUpdate();
+        }
+    }
+
+    InputMapper::update(micros());
 
     InputMapper::sendReport();
 }