Gyro works; Trackpads are failing with gyro on

include/gyro.h

@@ -0,0 +1,51 @@
+#ifndef GYRO_H
+#define GYRO_H
+
+#include <stdint.h>
+
+#include <MPU6050.h>
+
+class Gyro
+{
+    private:
+    int16_t x, y, z;
+
+    class Filter
+    {
+        private:
+        uint8_t size;
+        int16_t *buffer;
+        uint8_t pointer;
+        int32_t sum;
+
+        public:
+
+        void init(uint8_t size);
+
+        ~Filter();
+
+        int16_t filter(int16_t x);
+    };
+
+    Filter x_filter, y_filter, z_filter;
+
+    MPU6050 mpu;
+
+    bool (*_Enabled)();
+
+    public:
+
+    void init();
+
+    void setEnabledCallback(bool (*_Enabled)());
+    bool Enabled() { return _Enabled(); };
+
+    void update();
+
+    int16_t getX() { return x; }
+    int16_t getY() { return y; }
+    int16_t getZ() { return z; }
+
+};
+
+#endif

include/input_mapper.h

@@ -9,7 +9,7 @@ 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);
+    void update(uint32_t time, bool &gyro_ready);
 
     void mapTriggers(uint32_t value[2]);
 
@@ -32,9 +32,6 @@ namespace InputMapper
 
     bool mapButton(HardwareButtons button, bool value);
 
-    bool gyroEnabled();
-    void mapGyro(int16_t x, int16_t y, int16_t z);
-
     void sendReport();
 }
 

src/gyro.cpp

@@ -0,0 +1,78 @@
+#include "gyro.h"
+
+#include <stdint.h>
+ 
+void Gyro::Filter::init(uint8_t size)
+{
+    this->size = size;
+    buffer = new int16_t[size];
+    for (uint8_t i = 0; i < size; i++)
+    {
+        buffer[i] = 0;
+    }
+    pointer = 0;
+    sum = 0;
+}
+
+Gyro::Filter::~Filter()
+{
+    delete[] buffer;
+}
+
+int16_t Gyro::Filter::filter(int16_t x)
+{
+    sum -= buffer[pointer];
+    sum += x;
+    buffer[pointer] = x;
+
+    pointer = (pointer + 1) % size;
+
+    return sum / size;
+}
+
+void Gyro::init()
+{
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+        Wire.begin();
+    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+        Fastwire::setup(400, true);
+    #endif
+
+    mpu.initialize();
+    Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+
+    /*
+    mpu.setXGyroOffset(0);
+    mpu.setYGyroOffset(0);
+    mpu.setZGyroOffset(0);
+    */
+    mpu.CalibrateGyro();
+
+    uint8_t filter_size = 5;
+    x_filter.init(filter_size);
+    y_filter.init(filter_size);
+    z_filter.init(filter_size);
+
+    mpu.setIntDataReadyEnabled(1);
+
+    mpu.setDLPFMode(MPU6050_DLPF_BW_5);
+
+    _Enabled = [] { return false; };
+}
+
+void Gyro::setEnabledCallback(bool (*Enabled)())
+{
+    this->_Enabled = Enabled;
+}
+
+void Gyro::update()
+{
+    if (Enabled())
+    {
+        mpu.getRotation(&x, &y, &z);
+
+        x = x_filter.filter(x);
+        y = y_filter.filter(y);
+        z = z_filter.filter(z);
+    }
+}

src/input_mapper.cpp

@@ -2,6 +2,7 @@
 
 #include "usb_device.h"
 #include "touch_controls_all.h"
+#include "gyro.h"
 
 #include <map>
 
@@ -29,6 +30,9 @@ namespace InputMapper
     };
 
     const uint8_t num_controls = sizeof(tcontrols) / sizeof(TouchControl*[2]);
+
+    Gyro gyro;
+
     /*
     uint16_t button_map[] =
     {
@@ -170,6 +174,10 @@ namespace InputMapper
             }
         }
 
+        gyro.init();
+        //gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; });
+        gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; });
+
         device.begin();
     }
 
@@ -246,7 +254,7 @@ namespace InputMapper
         }
     }
 
-    void update(uint32_t time)
+    void update(uint32_t time, bool &gyro_ready)
     {
         for (uint8_t id = 0; id < 2; ++id)
         {
@@ -268,6 +276,12 @@ namespace InputMapper
 
             }
         }
+
+        if (gyro_ready)
+        {
+            gyro.update();
+            gyro_ready = false;
+        }
     }
 
     void mapTriggers(uint32_t value[2])
@@ -329,13 +343,7 @@ namespace InputMapper
         return res;
     }
 
-    bool gyroEnabled()
-    {
-        // how do I map this?
-        return tjoystick_right.getTouching() > TouchControl::CT_NONE;
-    }
-
-    void mapGyro(int16_t x, int16_t y, int16_t z)
+    void mapGyro()
     {
         //device.joystick(1, x, y);
     }
@@ -394,6 +402,12 @@ namespace InputMapper
             }
         }
 
+        if (gyro.Enabled())
+        {
+            dx[1] += gyro.getX();
+            dy[1] += gyro.getY();
+        }
+
         for (int j = 0; j < 2; ++j)
         {
             if (count[j] > 0)

src/main.cpp

@@ -1,9 +1,5 @@
 #include <Arduino.h>
 
-#include <MPU6050.h>
-
-MPU6050 mpu;
-
 #include "trackpad.h"
 #include "input_mapper.h"
 
@@ -29,10 +25,17 @@ uint8_t button_state[sizeof(pin_button) / 2] = {0};
 const uint8_t pin_trackpad_data[2]  = {PB5, PB9};
 const uint8_t pin_trackpad_clock[2] = {PB4, PB8};
 
+const uint8_t gyro_int = PC14;
+
 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
@@ -59,22 +62,8 @@ void setup()
 
     trackpad_maxX = trackpad[0].getMaxX();
     trackpad_maxY = trackpad[0].getMaxY();
-    
-    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
-        Wire.begin();
-    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
-        Fastwire::setup(400, true);
-    #endif
-
-    mpu.initialize();
-    Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
-
-    /*
-    mpu.setXGyroOffset(0);
-    mpu.setYGyroOffset(0);
-    mpu.setZGyroOffset(0);
-    */
-    mpu.CalibrateGyro();
+
+    attachInterrupt(gyro_int, dmpDataReady, RISING);
 
     InputMapper::begin();
 
@@ -150,14 +139,7 @@ void loop()
         }
     }
 
-    if (InputMapper::gyroEnabled())
-    {
-        int16_t x, y, z;
-        mpu.getRotation(&x, &y, &z);
-        InputMapper::mapGyro(x, y, z);
-    }
-
-    InputMapper::update(micros());
+    InputMapper::update(micros(), mpuInterrupt);
 
     InputMapper::sendReport();
 }