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

Выполняется корректное чтение данных иму. Оси и наклоны соответствуют рабочим прошивке и дрону

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-04-06 16:13:16 +03:00
parent 699577d82b
commit b713794a26
8 changed files with 100 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,13 +4,13 @@
static float gyro_bias_x = 0.0f;
/*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 float accel_bias_z = 0.0f;*/
static biquad_t accel_x_lpf;
static biquad_t accel_y_lpf;
@@ -20,8 +20,8 @@ static biquad_t gyro_x_lpf;
static biquad_t gyro_y_lpf;
static biquad_t gyro_z_lpf;
const float accel_min = -8192.0f;
const float accel_max = 8192.0f;
const float accel_min = -4096.0f;
const float accel_max = 4096.0f;
static int16_t x_calib;
static int16_t y_calib;
@@ -46,25 +46,15 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
imu_read_raw(&raw);
raw.ax = biquad_apply(&accel_x_lpf, raw.ax);
raw.ay = biquad_apply(&accel_y_lpf, raw.ay);
raw.az = biquad_apply(&accel_z_lpf, raw.az);
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 = biquad_apply(&gyro_x_lpf, raw.gx);
raw.gy = biquad_apply(&gyro_y_lpf, raw.gy);
raw.gz = biquad_apply(&gyro_z_lpf, raw.gz);
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, 8192, 1000);
/*float alpha = 0.01;
out->ax = out->ax * (1.0f - alpha) + raw.ax * alpha;
out->ay = out->ay * (1.0f - alpha) + raw.ay * alpha;
out->az = out->az * (1.0f - alpha) + raw.az * alpha;
out->gx = out->gx * (1.0f - alpha) + raw.gx * alpha;
out->gy = out->gy * (1.0f - alpha) + raw.gy * alpha;
out->gz = out->gz * (1.0f - alpha) + raw.gz * alpha;*/
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);
@@ -75,21 +65,38 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
////////////////////////////////////////////////////////////
/*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;
/*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->ax = biquad_apply(&accel_x_lpf, out->ax);
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 = 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);*/
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)
@@ -118,7 +125,13 @@ void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
acc = acc * (1.0f - alpha) + len * alpha;
}
void imu_calibrate()
uint16_t Rev16(uint16_t v)
{
asm("REV16 %1, %0" : "=r" (v) : "r" (v));
return v;
}
/*void imu_calibrate()
{
const int samples = 1000;
@@ -136,25 +149,25 @@ void imu_calibrate()
{
imu_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_ax += imu.ax;
sum_ay += imu.ay;
sum_az += imu.az;
sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
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) + 1.0f;
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)
{