diff --git a/include/gyro.h b/include/gyro.h new file mode 100644 index 0000000..f389123 --- /dev/null +++ b/include/gyro.h @@ -0,0 +1,51 @@ +#ifndef GYRO_H +#define GYRO_H + +#include + +#include + +class Gyro +{ + private: + int16_t x, y, z; + + 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; + + MPU6050 mpu; + + bool (*_Enabled)(); + + public: + + void init(); + + void setEnabledCallback(bool (*_Enabled)()); + bool Enabled() { return _Enabled(); }; + + void update(); + + int16_t getX() { return x; } + int16_t getY() { return y; } + int16_t getZ() { return z; } + +}; + +#endif \ No newline at end of file diff --git a/include/input_mapper.h b/include/input_mapper.h index fec6fc0..015874a 100644 --- a/include/input_mapper.h +++ b/include/input_mapper.h @@ -9,7 +9,7 @@ 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); - void update(uint32_t time); + void update(uint32_t time, bool &gyro_ready); void mapTriggers(uint32_t value[2]); @@ -32,9 +32,6 @@ namespace InputMapper bool mapButton(HardwareButtons button, bool value); - bool gyroEnabled(); - void mapGyro(int16_t x, int16_t y, int16_t z); - void sendReport(); } diff --git a/src/gyro.cpp b/src/gyro.cpp new file mode 100644 index 0000000..f65046a --- /dev/null +++ b/src/gyro.cpp @@ -0,0 +1,78 @@ +#include "gyro.h" + +#include + +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; +} + +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(); + + 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); + + _Enabled = [] { return false; }; +} + +void Gyro::setEnabledCallback(bool (*Enabled)()) +{ + this->_Enabled = Enabled; +} + +void Gyro::update() +{ + if (Enabled()) + { + mpu.getRotation(&x, &y, &z); + + x = x_filter.filter(x); + y = y_filter.filter(y); + z = z_filter.filter(z); + } +} diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp index 83219fe..201c549 100644 --- a/src/input_mapper.cpp +++ b/src/input_mapper.cpp @@ -2,6 +2,7 @@ #include "usb_device.h" #include "touch_controls_all.h" +#include "gyro.h" #include @@ -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,10 @@ namespace InputMapper } } + gyro.init(); + //gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; }); + gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; }); + device.begin(); } @@ -246,7 +254,7 @@ namespace InputMapper } } - void update(uint32_t time) + void update(uint32_t time, bool &gyro_ready) { for (uint8_t id = 0; id < 2; ++id) { @@ -268,6 +276,12 @@ namespace InputMapper } } + + if (gyro_ready) + { + gyro.update(); + gyro_ready = false; + } } void mapTriggers(uint32_t value[2]) @@ -329,13 +343,7 @@ namespace InputMapper return res; } - bool gyroEnabled() - { - // how do I map this? - return tjoystick_right.getTouching() > TouchControl::CT_NONE; - } - - void mapGyro(int16_t x, int16_t y, int16_t z) + void mapGyro() { //device.joystick(1, x, y); } @@ -394,6 +402,12 @@ namespace InputMapper } } + if (gyro.Enabled()) + { + dx[1] += gyro.getX(); + dy[1] += gyro.getY(); + } + for (int j = 0; j < 2; ++j) { if (count[j] > 0) diff --git a/src/main.cpp b/src/main.cpp index 1431252..91da62d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,9 +1,5 @@ #include -#include - -MPU6050 mpu; - #include "trackpad.h" #include "input_mapper.h" @@ -29,10 +25,17 @@ 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; +bool mpuInterrupt = false; +void dmpDataReady() { + mpuInterrupt = true; +} + void setup() { // Turn on LED @@ -59,22 +62,8 @@ void setup() trackpad_maxX = trackpad[0].getMaxX(); trackpad_maxY = trackpad[0].getMaxY(); - - #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(); + + attachInterrupt(gyro_int, dmpDataReady, RISING); InputMapper::begin(); @@ -150,14 +139,7 @@ void loop() } } - if (InputMapper::gyroEnabled()) - { - int16_t x, y, z; - mpu.getRotation(&x, &y, &z); - InputMapper::mapGyro(x, y, z); - } - - InputMapper::update(micros()); + InputMapper::update(micros(), mpuInterrupt); InputMapper::sendReport(); }