Merge branch 'gyro'
commit
7634060310
@ -0,0 +1,90 @@
|
||||
#ifndef GYRO_H
|
||||
#define GYRO_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <MPU6050.h>
|
||||
|
||||
class Gyro
|
||||
{
|
||||
private:
|
||||
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;
|
||||
|
||||
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
|
||||
{
|
||||
BIND_X,
|
||||
BIND_Z,
|
||||
BIND_XZ,
|
||||
};
|
||||
private:
|
||||
|
||||
BindToX bind_to_x;
|
||||
|
||||
MPU6050 mpu;
|
||||
|
||||
uint32_t time0;
|
||||
uint32_t delay;
|
||||
bool (*_Enabled)();
|
||||
|
||||
public:
|
||||
|
||||
Gyro();
|
||||
|
||||
void init();
|
||||
|
||||
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 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; }
|
||||
|
||||
void update(uint32_t time);
|
||||
|
||||
int16_t getX() { return x; }
|
||||
int16_t getY() { return y; }
|
||||
int16_t getZ() { return z; }
|
||||
|
||||
int16_t getDX();
|
||||
int16_t getDY();
|
||||
};
|
||||
|
||||
#endif
|
@ -0,0 +1,160 @@
|
||||
#include "gyro.h"
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "util_func.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;
|
||||
}
|
||||
|
||||
Gyro::Gyro()
|
||||
{
|
||||
invert_x = 1;
|
||||
invert_y = 1;
|
||||
invert_z = 1;
|
||||
|
||||
sensitivity = 1.0f;
|
||||
|
||||
time0 = 0;
|
||||
delay = 0;
|
||||
|
||||
bind_to_x = BIND_X;
|
||||
|
||||
_Enabled = [] { return false; };
|
||||
}
|
||||
|
||||
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(6);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void Gyro::update(uint32_t time)
|
||||
{
|
||||
if (Enabled())
|
||||
{
|
||||
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;
|
||||
|
||||
dt /= 1000.0f;
|
||||
|
||||
this->x = x * sensitivity * invert_x * dt;
|
||||
this->y = y * sensitivity * invert_y * dt;
|
||||
this->z = z * sensitivity * invert_z * dt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int16_t Gyro::getDX()
|
||||
{
|
||||
int32_t dx;
|
||||
|
||||
switch (bind_to_x)
|
||||
{
|
||||
case BIND_X:
|
||||
dx = x;
|
||||
break;
|
||||
|
||||
case BIND_Z:
|
||||
dx = z;
|
||||
break;
|
||||
|
||||
case BIND_XZ:
|
||||
dx = x + z;
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
|
||||
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);
|
||||
}
|
Loading…
Reference in New Issue