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
     {
@@ -65,6 +69,10 @@ class Gyro
     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; }
@@ -75,7 +83,7 @@ class Gyro
     int16_t getY() { return y; }
     int16_t getZ() { return z; }
 
-    int32_t getDX();
+    int16_t getDX();
     int16_t getDY();
 };
 

src/gyro.cpp

@@ -38,7 +38,14 @@ Gyro::Gyro()
     invert_y = 1;
     invert_z = 1;
 
+    sensitivity = 1.0f;
+
     time0 = 0;
+    delay = 0;
+
+    bind_to_x = BIND_X;
+
+    _Enabled = [] { return false; };
 }
 
 void Gyro::init()
@@ -57,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);
@@ -67,28 +74,35 @@ void Gyro::init()
     //mpu.setIntDataReadyEnabled(1);
 
     //mpu.setDLPFMode(MPU6050_DLPF_BW_5);
-
-    _Enabled = [] { return false; };
 }
 
 void Gyro::update(uint32_t time)
 {
     if (Enabled())
     {
-        if (time - time0 > delay)
+        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;
 
@@ -103,13 +117,44 @@ int32_t Gyro::getDX()
             break;
 
         case BIND_XZ:
-            dx = (int32_t)x + (int32_t)z;
+            dx = x + z;
     }
-
-    return dx;
+    
+    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

@@ -180,6 +180,9 @@ namespace InputMapper
         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);