diff --git a/include/gyro.h b/include/gyro.h index f389123..aa07b0b 100644 --- a/include/gyro.h +++ b/include/gyro.h @@ -8,8 +8,6 @@ class Gyro { private: - int16_t x, y, z; - class Filter { private: @@ -29,23 +27,52 @@ class Gyro Filter x_filter, y_filter, z_filter; + int16_t x, y, z; + uint8_t mapped_id; + + int8_t invert_x, invert_y, invert_z; + + public: + enum BindToX : uint8_t + { + BIND_X, + BIND_Z, + BIND_XZ, + }; + private: + + BindToX bind_to_x; + MPU6050 mpu; bool (*_Enabled)(); public: + Gyro(); + void init(); - void setEnabledCallback(bool (*_Enabled)()); + 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 setBindToX(BindToX bind_to_x) { this->bind_to_x = bind_to_x; }; + void update(); int16_t getX() { return x; } int16_t getY() { return y; } int16_t getZ() { return z; } + int32_t getDX(); + int16_t getDY(); }; #endif \ No newline at end of file diff --git a/src/gyro.cpp b/src/gyro.cpp index 02509d6..1022d39 100644 --- a/src/gyro.cpp +++ b/src/gyro.cpp @@ -1,6 +1,8 @@ #include "gyro.h" #include + +#include "util_func.h" void Gyro::Filter::init(uint8_t size) { @@ -30,6 +32,13 @@ int16_t Gyro::Filter::filter(int16_t x) return sum / size; } +Gyro::Gyro() +{ + invert_x = 1; + invert_y = 1; + invert_z = 1; +} + void Gyro::init() { #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE @@ -60,16 +69,33 @@ void Gyro::init() _Enabled = [] { return false; }; } -void Gyro::setEnabledCallback(bool (*Enabled)()) +void Gyro::update() { - this->_Enabled = Enabled; + 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; } -void Gyro::update() +int32_t Gyro::getDX() { - mpu.getRotation(&x, &y, &z); + switch (bind_to_x) + { + case BIND_X: + return x; + + case BIND_Z: + return z; + + case BIND_XZ: + return (int32_t)x + (int32_t)z; + } + + return 0; +} - x = x_filter.filter(x); - y = y_filter.filter(y); - z = z_filter.filter(z); +int16_t Gyro::getDY() +{ + return y; } diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp index 1cc965b..add3afd 100644 --- a/src/input_mapper.cpp +++ b/src/input_mapper.cpp @@ -177,6 +177,10 @@ 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.setBindToX(Gyro::BIND_XZ); device.begin(); } @@ -347,11 +351,6 @@ namespace InputMapper return res; } - void mapGyro() - { - //device.joystick(1, x, y); - } - void sendReport() { for (auto it = xinput_counter.begin(); it != xinput_counter.end(); ++it) @@ -408,8 +407,8 @@ namespace InputMapper if (gyro.Enabled()) { - dx[1] += gyro.getX(); - dy[1] += gyro.getY(); + dx[gyro.getMappedId()] += gyro.getDX(); + dy[gyro.getMappedId()] += gyro.getDY(); } for (int j = 0; j < 2; ++j) @@ -418,16 +417,17 @@ namespace InputMapper { 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();