92 lines
2.3 KiB
C
92 lines
2.3 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;
|
|
|
|
void imu_processing_init()
|
|
{
|
|
biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
|
|
biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
|
|
biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
|
|
|
|
biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
|
|
biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
|
|
biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
|
|
}
|
|
|
|
void imu_read_scaled(imu_scaled_t* out)
|
|
{
|
|
static imu_raw_t raw;
|
|
|
|
icm_read_raw(&raw);
|
|
|
|
out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
|
|
out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
|
|
out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_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 = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
|
|
out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
|
|
out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
|
|
|
|
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);
|
|
}
|
|
|
|
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)
|
|
{
|
|
icm_read_raw(&imu);
|
|
|
|
sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
|
|
sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
|
|
sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
|
|
|
|
sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
|
|
sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
|
|
sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
|
|
|
|
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) + 1.0f;
|
|
|
|
gyro_bias_x = sum_gx / samples;
|
|
gyro_bias_y = sum_gy / samples;
|
|
gyro_bias_z = sum_gz / samples;
|
|
}
|