Merge branch 'gyro'

include/gyro.h

@@ -0,0 +1,90 @@
+#ifndef GYRO_H
+#define GYRO_H
+
+#include <stdint.h>
+
+#include <MPU6050.h>
+
+class Gyro
+{
+    private:
+    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;
+
+    int32_t x, y, z;
+    uint8_t mapped_id;
+
+    int8_t invert_x, invert_y, invert_z;
+
+    float sensitivity;
+    int16_t deadzone;
+    int16_t min_delta;
+
+    public:
+    enum BindToX : uint8_t
+    {
+        BIND_X,
+        BIND_Z,
+        BIND_XZ,
+    };
+    private:
+
+    BindToX bind_to_x;
+
+    MPU6050 mpu;
+
+    uint32_t time0;
+    uint32_t delay;
+    bool (*_Enabled)();
+
+    public:
+
+    Gyro();
+
+    void init();
+
+    void setEnabledCallback(bool (*_Enabled)()) { this->_Enabled = _Enabled; }
+    bool Enabled() { return _Enabled(); }
+
+    void setMappedId(uint8_t mapped_id) { this->mapped_id = mapped_id; }
+    uint8_t getMappedId() { return mapped_id; }
+
+    void setInvertX(bool invert_x = true) { this->invert_x = invert_x? -1 : 1; }
+    void setInvertY(bool invert_y = true) { this->invert_y = invert_y? -1 : 1; }
+    void setInvertZ(bool invert_z = true) { this->invert_z = invert_z? -1 : 1; }
+
+    void setSensitivity(float sensitivity) { this->sensitivity = sensitivity; }
+    void setDeadzone(int16_t deadzone) { this->deadzone = deadzone; }
+    void setMinDelta(int16_t min_delta) { this->min_delta = min_delta; }
+
+    void setBindToX(BindToX bind_to_x) { this->bind_to_x = bind_to_x; }
+
+    void setDelay(uint32_t delay) { this->delay = delay; }
+
+    void update(uint32_t time);
+
+    int16_t getX() { return x; }
+    int16_t getY() { return y; }
+    int16_t getZ() { return z; }
+
+    int16_t getDX();
+    int16_t getDY();
+};
+
+#endif

include/input_mapper.h

@@ -32,8 +32,6 @@ namespace InputMapper
 
     bool mapButton(HardwareButtons button, bool value);
 
-    // void mapGyro();
-
     void sendReport();
 }
 

src/gyro.cpp

@@ -0,0 +1,160 @@
+#include "gyro.h"
+
+#include <stdint.h>
+
+#include "util_func.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;
+}
+
+Gyro::Gyro()
+{
+    invert_x = 1;
+    invert_y = 1;
+    invert_z = 1;
+
+    sensitivity = 1.0f;
+
+    time0 = 0;
+    delay = 0;
+
+    bind_to_x = BIND_X;
+
+    _Enabled = [] { return false; };
+}
+
+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(6);
+
+    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);
+}
+
+void Gyro::update(uint32_t time)
+{
+    if (Enabled())
+    {
+        float dt = time - time0;
+        if (dt > delay)
+        {
+            time0 = time;
+
+            int16_t x, y, z;
+
+            mpu.getRotation(&x, &y, &z);
+
+            //x = x_filter.filter(x) * invert_x;
+            //y = y_filter.filter(y) * invert_y;
+            //z = z_filter.filter(z) * invert_z;
+
+            dt /= 1000.0f;
+
+            this->x = x * sensitivity * invert_x * dt;
+            this->y = y * sensitivity * invert_y * dt;
+            this->z = z * sensitivity * invert_z * dt;
+        }
+    }
+}
+
+int16_t Gyro::getDX()
+{
+    int32_t dx;
+
+    switch (bind_to_x)
+    {
+        case BIND_X:
+            dx = x;
+            break;
+
+        case BIND_Z:
+            dx = z;
+            break;
+
+        case BIND_XZ:
+            dx = x + z;
+    }
+    
+    if (dx > -deadzone && dx < deadzone)
+    {
+        dx = 0;
+    }
+    else
+    if (dx >= deadzone && dx < min_delta)
+    {
+        dx = min_delta;
+    }
+    else
+    if (dx <= -deadzone && dx > -min_delta)
+    {
+        dx = -min_delta;
+    }
+    
+    return clamp(dx, -32767, 32767);
+}
+
+int16_t Gyro::getDY()
+{
+    
+    if (y > -deadzone && y < deadzone)
+    {
+        y = 0;
+    }
+    else
+    if (y >= deadzone && y < min_delta)
+    {
+        y = min_delta;
+    }
+    else
+    if (y <= -deadzone && y > -min_delta)
+    {
+        y = -min_delta;
+    }
+    
+    return clamp(y, -32767, 32767);
+}

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,18 @@ namespace InputMapper
             }
         }
 
+        gyro.init();
+        gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; });
+        //gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; });
+        gyro.setMappedId(1);
+        //gyro.setInvertX();
+        gyro.setInvertY();
+        gyro.setSensitivity(1.0f);
+        gyro.setDeadzone(0);
+        gyro.setMinDelta(1000);
+        gyro.setBindToX(Gyro::BIND_XZ);
+        gyro.setDelay(1000);
+
         device.begin();
     }
 
@@ -268,6 +284,8 @@ namespace InputMapper
 
             }
         }
+
+        gyro.update(time);
     }
 
     void mapTriggers(uint32_t value[2])
@@ -383,22 +401,29 @@ namespace InputMapper
             }
         }
 
+        if (gyro.Enabled())
+        {
+            dx[gyro.getMappedId()] += gyro.getDX();
+            dy[gyro.getMappedId()] += gyro.getDY();
+        }
+
         for (int j = 0; j < 2; ++j)
         {
             if (count[j] > 0)
             {
                 x[j] = x[j] / count[j] + dx[j];
                 y[j] = y[j] / count[j] + dy[j];
-
-                x[j] = clamp(x[j], USB_Device::usb_joystick_x - USB_Device::usb_joystick_r, USB_Device::usb_joystick_x + USB_Device::usb_joystick_r);
-                y[j] = clamp(y[j], USB_Device::usb_joystick_y - USB_Device::usb_joystick_r, USB_Device::usb_joystick_y + USB_Device::usb_joystick_r);
-
-                device.joystick(j, x[j], y[j]);
             }
             else
             {
-                device.joystick(j, USB_Device::usb_joystick_x + dx[j], USB_Device::usb_joystick_y + dy[j]);
+                x[j] = USB_Device::usb_joystick_x + dx[j];
+                y[j] = USB_Device::usb_joystick_y + dy[j];
             }
+
+            x[j] = clamp(x[j], USB_Device::usb_joystick_x - USB_Device::usb_joystick_r, USB_Device::usb_joystick_x + USB_Device::usb_joystick_r);
+            y[j] = clamp(y[j], USB_Device::usb_joystick_y - USB_Device::usb_joystick_r, USB_Device::usb_joystick_y + USB_Device::usb_joystick_r);
+
+            device.joystick(j, x[j], y[j]);
         }
 
         device.sendReport();

src/main.cpp

@@ -25,6 +25,8 @@ 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;
@@ -55,7 +57,7 @@ void setup()
 
     trackpad_maxX = trackpad[0].getMaxX();
     trackpad_maxY = trackpad[0].getMaxY();
-    
+
     InputMapper::begin();
 
     // Turn off LED
@@ -114,8 +116,6 @@ void loop()
             }
         }
     }
-    
-    InputMapper::update(micros());
 
     uint32_t triggers[] = {analogRead(pin_trigger[0]), analogRead(pin_trigger[1])};
     InputMapper::mapTriggers(triggers);
@@ -132,5 +132,7 @@ void loop()
         }
     }
 
+    InputMapper::update(micros());
+
     InputMapper::sendReport();
 }