bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Реализовано асинхронное чтение IMU

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-04-09 11:51:55 +03:00
parent b713794a26
commit 8b697f903b
8 changed files with 209 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
Source/BSP/Src/imu_processing.c
View File

@@ -2,15 +2,8 @@
#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;
@@ -45,6 +38,9 @@ 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);
@@ -63,40 +59,35 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
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_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)
@@ -104,8 +95,8 @@ 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 = 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;
@@ -131,17 +122,12 @@ uint16_t Rev16(uint16_t v)
return v;
}
/*void imu_calibrate()
void imu_accel_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;
@@ -151,23 +137,13 @@ uint16_t Rev16(uint16_t v)
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);
for (volatile uint16_t i = 0; i < 5000; ++i) { asm volatile("NOP"); };
}
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)
{