From 98c7f2637b14f478a95f17ebc1194aefda8e60f8 Mon Sep 17 00:00:00 2001 From: NepEgor Date: Tue, 10 May 2022 18:53:40 +0600 Subject: [PATCH 1/7] gyro initial --- include/input_mapper.h | 2 +- src/input_mapper.cpp | 5 +++++ src/main.cpp | 25 +++++++++++++++++++++++-- 3 files changed, 29 insertions(+), 3 deletions(-) diff --git a/include/input_mapper.h b/include/input_mapper.h index b63fef8..1f34f16 100644 --- a/include/input_mapper.h +++ b/include/input_mapper.h @@ -32,7 +32,7 @@ namespace InputMapper bool mapButton(HardwareButtons button, bool value); - // void mapGyro(); + void mapGyro(int32_t x, int32_t y, int32_t z); void sendReport(); } diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp index 145a5f5..3b28c74 100644 --- a/src/input_mapper.cpp +++ b/src/input_mapper.cpp @@ -329,6 +329,11 @@ namespace InputMapper return res; } + void mapGyro(int32_t x, int32_t y, int32_t z) + { + device.joystick(1, x, y); + } + void sendReport() { for (auto it = xinput_counter.begin(); it != xinput_counter.end(); ++it) diff --git a/src/main.cpp b/src/main.cpp index d3be764..973ba3d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,5 +1,8 @@ #include +#include "Wire.h" +const uint8_t MPU_addr = 0x68; + #include "trackpad.h" #include "input_mapper.h" @@ -56,6 +59,12 @@ void setup() trackpad_maxX = trackpad[0].getMaxX(); trackpad_maxY = trackpad[0].getMaxY(); + Wire.begin(); + Wire.beginTransmission(MPU_addr); + Wire.write(0x6B); // PWR_MGMT_1 register + Wire.write(0); // set to zero (wakes up the MPU-6050) + Wire.endTransmission(true); + InputMapper::begin(); // Turn off LED @@ -115,8 +124,6 @@ void loop() } } - InputMapper::update(micros()); - uint32_t triggers[] = {analogRead(pin_trigger[0]), analogRead(pin_trigger[1])}; InputMapper::mapTriggers(triggers); @@ -132,5 +139,19 @@ void loop() } } + int16_t data[7]; + Wire.beginTransmission(MPU_addr); + Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) + Wire.endTransmission(false); + Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers + for (byte i = 0; i < 7; i++) { + data[i] = Wire.read() << 8 | Wire.read(); + } + + //InputMapper::mapAccel(data[0], data[1], data[2]); + InputMapper::mapGyro(data[4], data[5], data[6]); + + InputMapper::update(micros()); + InputMapper::sendReport(); } From 8c27bd0cd7759e9c891012242e360495db12bef1 Mon Sep 17 00:00:00 2001 From: NepEgor Date: Wed, 11 May 2022 00:10:13 +0600 Subject: [PATCH 2/7] MPU6050 library --- include/input_mapper.h | 3 ++- src/input_mapper.cpp | 10 ++++++++-- src/main.cpp | 40 +++++++++++++++++++++++----------------- 3 files changed, 33 insertions(+), 20 deletions(-) diff --git a/include/input_mapper.h b/include/input_mapper.h index 1f34f16..fec6fc0 100644 --- a/include/input_mapper.h +++ b/include/input_mapper.h @@ -32,7 +32,8 @@ namespace InputMapper bool mapButton(HardwareButtons button, bool value); - void mapGyro(int32_t x, int32_t y, int32_t z); + bool gyroEnabled(); + void mapGyro(int16_t x, int16_t y, int16_t z); void sendReport(); } diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp index 3b28c74..83219fe 100644 --- a/src/input_mapper.cpp +++ b/src/input_mapper.cpp @@ -329,9 +329,15 @@ namespace InputMapper return res; } - void mapGyro(int32_t x, int32_t y, int32_t z) + bool gyroEnabled() { - device.joystick(1, x, y); + // how do I map this? + return tjoystick_right.getTouching() > TouchControl::CT_NONE; + } + + void mapGyro(int16_t x, int16_t y, int16_t z) + { + //device.joystick(1, x, y); } void sendReport() diff --git a/src/main.cpp b/src/main.cpp index 973ba3d..1431252 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,8 @@ #include -#include "Wire.h" -const uint8_t MPU_addr = 0x68; +#include + +MPU6050 mpu; #include "trackpad.h" #include "input_mapper.h" @@ -59,11 +60,21 @@ void setup() trackpad_maxX = trackpad[0].getMaxX(); trackpad_maxY = trackpad[0].getMaxY(); - Wire.begin(); - Wire.beginTransmission(MPU_addr); - Wire.write(0x6B); // PWR_MGMT_1 register - Wire.write(0); // set to zero (wakes up the MPU-6050) - Wire.endTransmission(true); + #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(); InputMapper::begin(); @@ -139,18 +150,13 @@ void loop() } } - int16_t data[7]; - Wire.beginTransmission(MPU_addr); - Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) - Wire.endTransmission(false); - Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers - for (byte i = 0; i < 7; i++) { - data[i] = Wire.read() << 8 | Wire.read(); + if (InputMapper::gyroEnabled()) + { + int16_t x, y, z; + mpu.getRotation(&x, &y, &z); + InputMapper::mapGyro(x, y, z); } - //InputMapper::mapAccel(data[0], data[1], data[2]); - InputMapper::mapGyro(data[4], data[5], data[6]); - InputMapper::update(micros()); InputMapper::sendReport(); From b4dcb63d84dd0e0354a0569c51a2278316706b76 Mon Sep 17 00:00:00 2001 From: NepEgor Date: Thu, 12 May 2022 19:51:34 +0600 Subject: [PATCH 3/7] Gyro works; Trackpads are failing with gyro on --- include/gyro.h | 51 +++++++++++++++++++++++++++ include/input_mapper.h | 5 +-- src/gyro.cpp | 78 ++++++++++++++++++++++++++++++++++++++++++ src/input_mapper.cpp | 30 +++++++++++----- src/main.cpp | 38 ++++++-------------- 5 files changed, 162 insertions(+), 40 deletions(-) create mode 100644 include/gyro.h create mode 100644 src/gyro.cpp 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(); } From c8cd5cd8df46c618b2f44a659730dd14ea3c0320 Mon Sep 17 00:00:00 2001 From: NepEgor Date: Sat, 14 May 2022 22:19:56 +0600 Subject: [PATCH 4/7] removed gyro int; fixed with ps/2 update --- include/input_mapper.h | 5 ++++- src/gyro.cpp | 15 ++++++--------- src/input_mapper.cpp | 22 +++++++++++++--------- src/main.cpp | 22 +++++++++++++--------- 4 files changed, 36 insertions(+), 28 deletions(-) diff --git a/include/input_mapper.h b/include/input_mapper.h index 015874a..49cbb79 100644 --- a/include/input_mapper.h +++ b/include/input_mapper.h @@ -9,7 +9,10 @@ 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, bool &gyro_ready); + bool gyroEnabled(); + void gyroUpdate(); + + void update(uint32_t time); void mapTriggers(uint32_t value[2]); diff --git a/src/gyro.cpp b/src/gyro.cpp index f65046a..02509d6 100644 --- a/src/gyro.cpp +++ b/src/gyro.cpp @@ -53,9 +53,9 @@ void Gyro::init() y_filter.init(filter_size); z_filter.init(filter_size); - mpu.setIntDataReadyEnabled(1); + //mpu.setIntDataReadyEnabled(1); - mpu.setDLPFMode(MPU6050_DLPF_BW_5); + //mpu.setDLPFMode(MPU6050_DLPF_BW_5); _Enabled = [] { return false; }; } @@ -67,12 +67,9 @@ void Gyro::setEnabledCallback(bool (*Enabled)()) void Gyro::update() { - if (Enabled()) - { - mpu.getRotation(&x, &y, &z); + mpu.getRotation(&x, &y, &z); - x = x_filter.filter(x); - y = y_filter.filter(y); - z = z_filter.filter(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 201c549..1cc965b 100644 --- a/src/input_mapper.cpp +++ b/src/input_mapper.cpp @@ -175,8 +175,8 @@ namespace InputMapper } gyro.init(); - //gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; }); - gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; }); + gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; }); + //gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; }); device.begin(); } @@ -254,7 +254,17 @@ namespace InputMapper } } - void update(uint32_t time, bool &gyro_ready) + bool gyroEnabled() + { + return gyro.Enabled(); + } + + void gyroUpdate() + { + gyro.update(); + } + + void update(uint32_t time) { for (uint8_t id = 0; id < 2; ++id) { @@ -276,12 +286,6 @@ namespace InputMapper } } - - if (gyro_ready) - { - gyro.update(); - gyro_ready = false; - } } void mapTriggers(uint32_t value[2]) diff --git a/src/main.cpp b/src/main.cpp index 91da62d..175e255 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,11 +31,6 @@ 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 @@ -63,8 +58,6 @@ void setup() trackpad_maxX = trackpad[0].getMaxX(); trackpad_maxY = trackpad[0].getMaxY(); - attachInterrupt(gyro_int, dmpDataReady, RISING); - InputMapper::begin(); // Turn off LED @@ -123,7 +116,7 @@ void loop() } } } - + uint32_t triggers[] = {analogRead(pin_trigger[0]), analogRead(pin_trigger[1])}; InputMapper::mapTriggers(triggers); @@ -139,7 +132,18 @@ void loop() } } - InputMapper::update(micros(), mpuInterrupt); + 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(); } From 3a23dd5fc838d285167fd079920f4725a5d244fe Mon Sep 17 00:00:00 2001 From: NepEgor Date: Sat, 14 May 2022 23:12:45 +0600 Subject: [PATCH 5/7] invert axis; bind axis to X; bind joystick id --- include/gyro.h | 33 ++++++++++++++++++++++++++++++--- src/gyro.cpp | 40 +++++++++++++++++++++++++++++++++------- src/input_mapper.cpp | 26 +++++++++++++------------- 3 files changed, 76 insertions(+), 23 deletions(-) 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(); From 816887a1b8947c34297ef936a95c717ba91433d3 Mon Sep 17 00:00:00 2001 From: NepEgor Date: Sun, 15 May 2022 20:07:17 +0600 Subject: [PATCH 6/7] gyro update moved inside the class --- include/gyro.h | 18 +++++++++++------- include/input_mapper.h | 3 --- src/gyro.cpp | 32 +++++++++++++++++++++++--------- src/input_mapper.cpp | 15 ++++----------- src/main.cpp | 11 ----------- 5 files changed, 38 insertions(+), 41 deletions(-) diff --git a/include/gyro.h b/include/gyro.h index aa07b0b..eae91f3 100644 --- a/include/gyro.h +++ b/include/gyro.h @@ -45,6 +45,8 @@ class Gyro MPU6050 mpu; + uint32_t time0; + uint32_t delay; bool (*_Enabled)(); public: @@ -54,18 +56,20 @@ 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 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 update(); + void setDelay(uint32_t delay) { this->delay = delay; } + + void update(uint32_t time); int16_t getX() { return x; } int16_t getY() { return y; } diff --git a/include/input_mapper.h b/include/input_mapper.h index 49cbb79..fe60fb0 100644 --- a/include/input_mapper.h +++ b/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]); diff --git a/src/gyro.cpp b/src/gyro.cpp index 1022d39..bd8307d 100644 --- a/src/gyro.cpp +++ b/src/gyro.cpp @@ -37,6 +37,8 @@ Gyro::Gyro() invert_x = 1; invert_y = 1; invert_z = 1; + + time0 = 0; } void Gyro::init() @@ -69,30 +71,42 @@ void Gyro::init() _Enabled = [] { return false; }; } -void Gyro::update() +void Gyro::update(uint32_t time) { - mpu.getRotation(&x, &y, &z); + if (Enabled()) + { + if (time - time0 > delay) + { + time0 = time; + + 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; + } + } } int32_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 = (int32_t)x + (int32_t)z; } - return 0; + return dx; } int16_t Gyro::getDY() diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp index add3afd..2f7524e 100644 --- a/src/input_mapper.cpp +++ b/src/input_mapper.cpp @@ -178,9 +178,10 @@ 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.setBindToX(Gyro::BIND_XZ); + gyro.setDelay(1000); device.begin(); } @@ -258,16 +259,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 +281,8 @@ namespace InputMapper } } + + gyro.update(time); } void mapTriggers(uint32_t value[2]) diff --git a/src/main.cpp b/src/main.cpp index 175e255..a532a9d 100644 --- a/src/main.cpp +++ b/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(); From f33c4fd47fd72acd5f526797016a96e3aeeb16fb Mon Sep 17 00:00:00 2001 From: NepEgor Date: Wed, 25 May 2022 17:49:06 +0300 Subject: [PATCH 7/7] added sensitivity, deadzone and min_delta; removed filter use --- include/gyro.h | 12 ++++++-- src/gyro.cpp | 69 ++++++++++++++++++++++++++++++++++++-------- src/input_mapper.cpp | 3 ++ 3 files changed, 70 insertions(+), 14 deletions(-) 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);