added sensitivity, deadzone and min_delta; removed filter use

include/gyro.h

@@ -27,11 +27,15 @@ class Gyro
 
     Filter x_filter, y_filter, z_filter;
 
-    int16_t x, y, z;
+    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
     {
@@ -45,6 +49,8 @@ class Gyro
 
     MPU6050 mpu;
 
+    uint32_t time0;
+    uint32_t delay;
     bool (*_Enabled)();
 
     public:
@@ -54,24 +60,30 @@ class Gyro
     void init();
 
     void setEnabledCallback(bool (*_Enabled)()) { this->_Enabled = _Enabled; }
-    bool Enabled() { return _Enabled(); };
+    bool Enabled() { return _Enabled(); }
 
-    void setMappedId(uint8_t mapped_id) { this->mapped_id = mapped_id; };
+    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 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 setBindToX(BindToX bind_to_x) { this->bind_to_x = bind_to_x; };
+    void setDelay(uint32_t delay) { this->delay = delay; }
 
-    void update();
+    void update(uint32_t time);
 
     int16_t getX() { return x; }
     int16_t getY() { return y; }
     int16_t getZ() { return z; }
 
-    int32_t getDX();
+    int16_t getDX();
     int16_t getDY();
 };
 

include/input_mapper.h

@@ -9,9 +9,6 @@ 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);
 
-    bool gyroEnabled();
-    void gyroUpdate();
-
     void update(uint32_t time);
 
     void mapTriggers(uint32_t value[2]);

src/gyro.cpp

@@ -37,6 +37,15 @@ 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()
@@ -55,7 +64,7 @@ void Gyro::init()
     mpu.setYGyroOffset(0);
     mpu.setZGyroOffset(0);
     */
-    mpu.CalibrateGyro();
+    mpu.CalibrateGyro(6);
 
     uint8_t filter_size = 5;
     x_filter.init(filter_size);
@@ -65,37 +74,87 @@ void Gyro::init()
     //mpu.setIntDataReadyEnabled(1);
 
     //mpu.setDLPFMode(MPU6050_DLPF_BW_5);
-
-    _Enabled = [] { return false; };
 }
 
-void Gyro::update()
+void Gyro::update(uint32_t time)
 {
-    mpu.getRotation(&x, &y, &z);
+    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;
 
-    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;
+        }
+    }
 }
 
-int32_t Gyro::getDX()
+int16_t Gyro::getDX()
 {
+    int32_t dx;
+
     switch (bind_to_x)
     {
         case BIND_X:
-            return x;
+            dx = x;
+            break;
 
         case BIND_Z:
-            return z;
+            dx = z;
+            break;
 
         case BIND_XZ:
-            return (int32_t)x + (int32_t)z;
+            dx = x + z;
     }
-
-    return 0;
+    
+    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()
 {
-    return y;
+    
+    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

@@ -178,9 +178,13 @@ namespace InputMapper
         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.setInvertX();
         gyro.setInvertY();
+        gyro.setSensitivity(1.0f);
+        gyro.setDeadzone(0);
+        gyro.setMinDelta(1000);
         gyro.setBindToX(Gyro::BIND_XZ);
+        gyro.setDelay(1000);
 
         device.begin();
     }
@@ -258,16 +262,6 @@ namespace InputMapper
         }
     }
 
-    bool gyroEnabled()
-    {
-        return gyro.Enabled();
-    }
-
-    void gyroUpdate()
-    {
-        gyro.update();
-    }
-
     void update(uint32_t time)
     {
         for (uint8_t id = 0; id < 2; ++id)
@@ -290,6 +284,8 @@ namespace InputMapper
 
             }
         }
+
+        gyro.update(time);
     }
 
     void mapTriggers(uint32_t value[2])

src/main.cpp

@@ -132,17 +132,6 @@ void loop()
         }
     }
 
-    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();