184 lines
4.4 KiB
C
184 lines
4.4 KiB
C
#include "imu_processing.h"
|
|
#include "biquad_filter.h"
|
|
#include "math.h"
|
|
|
|
|
|
|
|
/*static float gyro_bias_x = 0.0f;
|
|
static float gyro_bias_y = 0.0f;
|
|
static float gyro_bias_z = 0.0f;
|
|
|
|
static float accel_bias_x = 0.0f;
|
|
static float accel_bias_y = 0.0f;
|
|
static float accel_bias_z = 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 = (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;
|
|
|
|
////////////////////////////////////////////////////////////
|
|
|
|
/*raw.ax = Rev16(raw.ax);
|
|
raw.ay = Rev16(raw.ay);
|
|
raw.az = Rev16(raw.az);
|
|
|
|
raw.gx = Rev16(raw.gx);
|
|
raw.gy = Rev16(raw.gy);
|
|
raw.gz = Rev16(raw.gz);
|
|
|
|
out->ax = raw.ax - accel_bias_x;
|
|
out->ay = raw.ay - accel_bias_y;
|
|
out->az = raw.az - accel_bias_z;
|
|
|
|
out->gx = raw.gx - gyro_bias_x;
|
|
out->gy = raw.gy - gyro_bias_y;
|
|
out->gz = raw.gz - gyro_bias_z;
|
|
|
|
out->ax = biquad_apply(&accel_x_lpf, out->ax);
|
|
out->ay = biquad_apply(&accel_y_lpf, out->ay);
|
|
out->az = biquad_apply(&accel_z_lpf, out->az);
|
|
|
|
out->gx = biquad_apply(&gyro_x_lpf, out->gx);
|
|
out->gy = biquad_apply(&gyro_y_lpf, out->gy);
|
|
out->gz = biquad_apply(&gyro_z_lpf, out->gz);
|
|
|
|
out->ax /= ACCEL_SENS_SCALE_FACTOR;
|
|
out->ay /= ACCEL_SENS_SCALE_FACTOR;
|
|
out->az /= ACCEL_SENS_SCALE_FACTOR;
|
|
|
|
out->gx /= GYRO_SENS_SCALE_FACTOR;
|
|
out->gy /= GYRO_SENS_SCALE_FACTOR;
|
|
out->gz /= 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;
|
|
|
|
x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz;
|
|
const float alpha = 0.001f;
|
|
|
|
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_calibrate()
|
|
{
|
|
const int samples = 1000;
|
|
|
|
float sum_ax = 0;
|
|
float sum_ay = 0;
|
|
float sum_az = 0;
|
|
|
|
float sum_gx = 0;
|
|
float sum_gy = 0;
|
|
float sum_gz = 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;
|
|
sum_az += imu.az;
|
|
|
|
sum_gx += imu.gx;
|
|
sum_gy += imu.gy;
|
|
sum_gz += imu.gz;
|
|
|
|
for (volatile uint16_t d = 0; d < 5000; ++d);
|
|
}
|
|
|
|
accel_bias_x = sum_ax / samples;
|
|
accel_bias_y = sum_ay / samples;
|
|
accel_bias_z = sum_az / samples;
|
|
|
|
gyro_bias_x = sum_gx / samples;
|
|
gyro_bias_y = sum_gy / samples;
|
|
gyro_bias_z = sum_gz / 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;
|
|
}
|