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

13
Source/BSP/Src/imu.c
View File

@@ -23,6 +23,9 @@ void i2c_gpio_init()
GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
// high speed
GPIOB->OSPEEDR |= (2 << (8 * 2)) | (2 << (9 * 2));
// enable open-drain
GPIOB->OTYPER |= (1 << 8) | (1 << 9);
@@ -36,8 +39,10 @@ void i2c1_init()
// enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 0x00303D5B - 100 kHz
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B;
I2C1->TIMINGR = 0x10802D9BUL;
I2C1->CR1 |= I2C_CR1_PE;
}
@@ -56,7 +61,7 @@ void imu_init()
// gyro ~2000 dps, FS_SEL = 3
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON);
// accel ~4g, FS_SEL = 1
// accel 8g, FS_SEL = 2
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON);
// back to bank 0
@@ -127,8 +132,8 @@ void imu_read_raw(imu_raw_t* data)
data->ay = (buf[2] << 8) | buf[3];
data->az = (buf[4] << 8) | buf[5];
data->gx = (buf[6] << 8) | buf[7];
data->gy = (buf[8] << 8) | buf[9];
data->gx = (buf[6] << 8) | buf[7];
data->gy = (buf[8] << 8) | buf[9];
data->gz = (buf[10] << 8) | buf[11];
}

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)
{