160 lines
4.1 KiB
C
160 lines
4.1 KiB
C
#include "imu_processing.h"
|
|
#include "biquad_filter.h"
|
|
#include "math.h"
|
|
|
|
static float accel_bias_x = 0.0f;
|
|
static float accel_bias_y = 0.0f;
|
|
|
|
static biquad_t accel_x_lpf;
|
|
static biquad_t accel_y_lpf;
|
|
static biquad_t accel_z_lpf;
|
|
|
|
static biquad_t gyro_x_lpf;
|
|
static biquad_t gyro_y_lpf;
|
|
static biquad_t gyro_z_lpf;
|
|
|
|
const float accel_min = -4096.0f;
|
|
const float accel_max = 4096.0f;
|
|
|
|
static int16_t x_calib;
|
|
static int16_t y_calib;
|
|
static int16_t z_calib;
|
|
static long gyro_shift[3] = {0, 0, 0};
|
|
static float acc;
|
|
|
|
void imu_processing_init()
|
|
{
|
|
biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f);
|
|
biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
|
|
biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
|
|
|
|
biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
|
|
biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
|
|
biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
|
|
}
|
|
|
|
void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
|
|
{
|
|
static imu_raw_t raw;
|
|
|
|
imu_read_raw(&raw);
|
|
|
|
raw.ax -= accel_bias_x;
|
|
raw.ay -= accel_bias_y;
|
|
|
|
raw.ax = (uint16_t) biquad_apply(&accel_x_lpf, raw.ax);
|
|
raw.ay = (uint16_t) biquad_apply(&accel_y_lpf, raw.ay);
|
|
raw.az = (uint16_t) biquad_apply(&accel_z_lpf, raw.az);
|
|
|
|
raw.gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw.gx);
|
|
raw.gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw.gy);
|
|
raw.gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw.gz);
|
|
|
|
if (allowed_calib) imu_calib(&raw, 50, 4096, 500);
|
|
|
|
out->ax = normalize(raw.ax, 1.0f, accel_min, accel_max);
|
|
out->ay = normalize(raw.ay, 1.0f, accel_min, accel_max);
|
|
out->az = normalize(raw.az, 1.0f, accel_min, accel_max);
|
|
|
|
out->gx = (raw.gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR;
|
|
out->gy = (raw.gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR;
|
|
out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
|
|
}
|
|
|
|
void imu_process_raw(imu_scaled_t* out, imu_raw_t* raw, const uint8_t* allowed_calib)
|
|
{
|
|
// bias
|
|
raw->ax -= accel_bias_x;
|
|
raw->ay -= accel_bias_y;
|
|
|
|
// фильтры
|
|
raw->ax = (uint16_t) biquad_apply(&accel_x_lpf, raw->ax);
|
|
raw->ay = (uint16_t) biquad_apply(&accel_y_lpf, raw->ay);
|
|
raw->az = (uint16_t) biquad_apply(&accel_z_lpf, raw->az);
|
|
|
|
raw->gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw->gx);
|
|
raw->gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw->gy);
|
|
raw->gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw->gz);
|
|
|
|
// калибровка
|
|
if (allowed_calib)
|
|
imu_calib(raw, 50, 4096, 500);
|
|
|
|
// масштабирование
|
|
out->ax = normalize(raw->ax, 1.0f, accel_min, accel_max);
|
|
out->ay = normalize(raw->ay, 1.0f, accel_min, accel_max);
|
|
out->az = normalize(raw->az, 1.0f, accel_min, accel_max);
|
|
|
|
out->gx = (raw->gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR;
|
|
out->gy = (raw->gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR;
|
|
out->gz = (raw->gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
|
|
}
|
|
|
|
void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
|
|
{
|
|
long len = imu->gx*imu->gx + imu->gy*imu->gy + imu->gz*imu->gz;
|
|
if (len > gyrLim*gyrLim) return;
|
|
|
|
const float alpha = 0.001f;
|
|
x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz;
|
|
|
|
x_calib = x_calib * (1.0f - alpha) + imu->gx * alpha;
|
|
y_calib = y_calib * (1.0f - alpha) + imu->gy * alpha;
|
|
z_calib = z_calib * (1.0f - alpha) + imu->gz * alpha;
|
|
|
|
gyro_shift[0] = x_calib;
|
|
gyro_shift[1] = y_calib;
|
|
gyro_shift[2] = z_calib;
|
|
|
|
len = imu->ax*imu->ax + imu->ay*imu->ay + imu->az*imu->az;
|
|
if (absl(len - accZero*accZero) > accMax*accMax) return;
|
|
|
|
len = sqrtf(len);
|
|
|
|
acc = len;
|
|
|
|
acc = acc * (1.0f - alpha) + len * alpha;
|
|
}
|
|
|
|
uint16_t Rev16(uint16_t v)
|
|
{
|
|
asm("REV16 %1, %0" : "=r" (v) : "r" (v));
|
|
return v;
|
|
}
|
|
|
|
void imu_accel_calibrate()
|
|
{
|
|
const int samples = 1000;
|
|
|
|
float sum_ax = 0;
|
|
float sum_ay = 0;
|
|
|
|
imu_raw_t imu;
|
|
|
|
for (uint16_t i = 0; i < samples; ++i)
|
|
{
|
|
imu_read_raw(&imu);
|
|
|
|
sum_ax += imu.ax;
|
|
sum_ay += imu.ay;
|
|
|
|
for (volatile uint16_t i = 0; i < 5000; ++i) { asm volatile("NOP"); };
|
|
}
|
|
|
|
accel_bias_x = sum_ax / samples;
|
|
accel_bias_y = sum_ay / samples;
|
|
}
|
|
|
|
float normalize(float value, float scale, float min, float max)
|
|
{
|
|
const float len = (max - min) / 2.0f;
|
|
const float shift = (max + min) / 2.0f;
|
|
return (value - shift) * scale / len;
|
|
}
|
|
|
|
long absl(long value)
|
|
{
|
|
if (value < 0) return -value;
|
|
return value;
|
|
}
|