From 98c7f2637b14f478a95f17ebc1194aefda8e60f8 Mon Sep 17 00:00:00 2001
From: NepEgor <enepochatyy@gmail.com>
Date: Tue, 10 May 2022 18:53:40 +0600
Subject: [PATCH] gyro initial

---
 include/input_mapper.h |  2 +-
 src/input_mapper.cpp   |  5 +++++
 src/main.cpp           | 25 +++++++++++++++++++++++--
 3 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/include/input_mapper.h b/include/input_mapper.h
index b63fef8..1f34f16 100644
--- a/include/input_mapper.h
+++ b/include/input_mapper.h
@@ -32,7 +32,7 @@ namespace InputMapper
 
     bool mapButton(HardwareButtons button, bool value);
 
-    // void mapGyro();
+    void mapGyro(int32_t x, int32_t y, int32_t z);
 
     void sendReport();
 }
diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp
index 145a5f5..3b28c74 100644
--- a/src/input_mapper.cpp
+++ b/src/input_mapper.cpp
@@ -329,6 +329,11 @@ namespace InputMapper
         return res;
     }
 
+    void mapGyro(int32_t x, int32_t y, int32_t z)
+    {
+        device.joystick(1, x, y);
+    }
+
     void sendReport()
     {   
         for (auto it = xinput_counter.begin(); it != xinput_counter.end(); ++it)
diff --git a/src/main.cpp b/src/main.cpp
index d3be764..973ba3d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,8 @@
 #include <Arduino.h>
 
+#include "Wire.h"
+const uint8_t MPU_addr = 0x68;
+
 #include "trackpad.h"
 #include "input_mapper.h"
 
@@ -56,6 +59,12 @@ void setup()
     trackpad_maxX = trackpad[0].getMaxX();
     trackpad_maxY = trackpad[0].getMaxY();
     
+    Wire.begin();
+    Wire.beginTransmission(MPU_addr);
+    Wire.write(0x6B);  // PWR_MGMT_1 register
+    Wire.write(0);     // set to zero (wakes up the MPU-6050)
+    Wire.endTransmission(true);
+
     InputMapper::begin();
 
     // Turn off LED
@@ -115,8 +124,6 @@ void loop()
         }
     }
     
-    InputMapper::update(micros());
-
     uint32_t triggers[] = {analogRead(pin_trigger[0]), analogRead(pin_trigger[1])};
     InputMapper::mapTriggers(triggers);
 
@@ -132,5 +139,19 @@ void loop()
         }
     }
 
+    int16_t data[7];
+    Wire.beginTransmission(MPU_addr);
+    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
+    Wire.endTransmission(false);
+    Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
+    for (byte i = 0; i < 7; i++) {
+        data[i] = Wire.read() << 8 | Wire.read();
+    }
+
+    //InputMapper::mapAccel(data[0], data[1], data[2]);
+    InputMapper::mapGyro(data[4], data[5], data[6]);
+
+    InputMapper::update(micros());
+
     InputMapper::sendReport();
 }