diff --git a/include/gyro.h b/include/gyro.h index eae91f3..184522c 100644 --- a/include/gyro.h +++ b/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(); }; diff --git a/src/gyro.cpp b/src/gyro.cpp index bd8307d..6a7a4a7 100644 --- a/src/gyro.cpp +++ b/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); } diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp index 2f7524e..9166421 100644 --- a/src/input_mapper.cpp +++ b/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);