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

15
Source/BSP/Inc/imu.h
View File

@@ -10,7 +10,8 @@
#define REG_BANK_SEL 0x7F
#define REG_GYRO_CONFIG_1 0x01
#define REG_ACCEL_CONFIG 0x14
#define IMU_DT 0.002f // 2 ms
#define FREQUENCY 100.0f
#define IMU_DT 1.0f / FREQUENCY
#define GYRO_FS_SEL_2000 3 << 1
#define GYRO_DLPFCFG_73 3 << 3
@@ -23,12 +24,20 @@
#define ACCEL_FCHOICE_ON 1
#define ACCEL_FCHOICE_OFF 0
static volatile uint8_t i2c_busy = 0;
static uint8_t i2c_buf[16];
static uint8_t i2c_len = 0;
static uint8_t i2c_reg = 0;
static uint8_t i2c_addr = 0;
typedef struct
{
int16_t ax, ay, az; // lsb
int16_t gx, gy, gz; // lsb
} imu_raw_t;
static void (*i2c_callback)(uint8_t* buf) = 0;
void imu_pow_init();
void i2c_gpio_init();
@@ -37,10 +46,12 @@ void i2c1_init();
void imu_init();
void imu_tim6_init();
void imu_tim6_init(const uint16_t freq);
void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
uint8_t i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*));
void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);
void imu_read_raw(imu_raw_t* data);

3
Source/BSP/Inc/imu_processing.h
View File

@@ -17,11 +17,12 @@ typedef struct
void imu_processing_init();
void imu_process_raw(imu_scaled_t* out, imu_raw_t* raw, const uint8_t* allowed_calib);
void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib);
void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax);
uint16_t Rev16(uint16_t v);
void imu_calibrate();
void imu_accel_calibrate();
float normalize(float value, float scale, float min, float max);
long absl(long value);

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

@@ -36,15 +36,12 @@ void i2c_gpio_init()
void i2c1_init()
{
// enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 0x00303D5B - 100 kHz
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x10802D9BUL;
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN; // enable I2C1
I2C1->CR1 |= I2C_CR1_PE;
I2C1->TIMINGR = 0x10802D9BUL; // 400 kHz @ 16 MHz
I2C1->CR1 |= I2C_CR1_PE;
}
void imu_init()
@@ -68,13 +65,14 @@ void imu_init()
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
}
void imu_tim6_init()
void imu_tim6_init(const uint16_t freq)
{
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
TIM6->PSC = 16000 - 1; // 16 MHz / 16000 = 1000 Hz (1 ms)
TIM6->ARR = 2 - 1; // 2 ms
TIM6->CR1 = 0;
TIM6->ARR = 1000 - 1;
TIM6->PSC = (SystemCoreClock / 1000 / freq) - 1;
TIM6->DIER |= TIM_DIER_UIE; // interrupt enable
TIM6->CR1 |= TIM_CR1_CEN; // counter enable
@@ -105,6 +103,27 @@ void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
I2C1->ICR |= I2C_ICR_STOPCF;
}
uint8_t i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*))
{
if (i2c_busy) return 0;
i2c_busy = 1;
i2c_addr = addr;
i2c_reg = reg;
i2c_len = len;
i2c_callback = cb;
// включаем прерывания
I2C1->CR1 |= I2C_CR1_TXIE | I2C_CR1_RXIE | I2C_CR1_TCIE | I2C_CR1_STOPIE;
NVIC_EnableIRQ(I2C1_EV_IRQn);
// старт записи регистра
I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
return 1;
}
void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
{
i2c_wait_idle(I2C1);
@@ -122,6 +141,51 @@ void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
I2C1->CR2 |= I2C_CR2_STOP;
}
void I2C1_EV_IRQHandler()
{
static uint8_t index = 0;
static uint8_t stage = 0; // 0 = send reg, 1 = read
// TX — отправляем регистр
if (I2C1->ISR & I2C_ISR_TXIS)
{
I2C1->TXDR = i2c_reg;
}
// переход к чтению
if (I2C1->ISR & I2C_ISR_TC)
{
I2C1->CR2 = (i2c_addr << 1) |
I2C_CR2_RD_WRN |
(i2c_len << I2C_CR2_NBYTES_Pos) |
I2C_CR2_AUTOEND |
I2C_CR2_START;
index = 0;
stage = 1;
}
// чтение данных
if (I2C1->ISR & I2C_ISR_RXNE)
{
i2c_buf[index++] = I2C1->RXDR;
}
// завершение
if (I2C1->ISR & I2C_ISR_STOPF)
{
I2C1->ICR |= I2C_ICR_STOPCF;
i2c_busy = 0;
// отключаем IRQ
I2C1->CR1 &= ~(I2C_CR1_TXIE | I2C_CR1_RXIE | I2C_CR1_TCIE | I2C_CR1_STOPIE);
if (i2c_callback)
i2c_callback(i2c_buf);
}
}
void imu_read_raw(imu_raw_t* data)
{
uint8_t buf[12];
@@ -139,7 +203,17 @@ void imu_read_raw(imu_raw_t* data)
static void i2c_wait_idle(I2C_TypeDef* i2c)
{
while (i2c->ISR & I2C_ISR_BUSY);
int timeout = 100000;
while ((i2c->ISR & I2C_ISR_BUSY) && --timeout);
if (timeout == 0)
{
// сброс I2C
i2c->CR1 &= ~I2C_CR1_PE;
i2c->CR1 |= I2C_CR1_PE;
}
// while (i2c->ISR & I2C_ISR_BUSY);
i2c->ICR = I2C_ICR_STOPCF |
I2C_ICR_NACKCF |

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