Gyro works; Trackpads are failing with gyro on
parent
8c27bd0cd7
commit
b4dcb63d84
@ -0,0 +1,51 @@
|
|||||||
|
#ifndef GYRO_H
|
||||||
|
#define GYRO_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include <MPU6050.h>
|
||||||
|
|
||||||
|
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
|
@ -0,0 +1,78 @@
|
|||||||
|
#include "gyro.h"
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
Loading…
Reference in New Issue