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

94
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
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN; // enable I2C1
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x10802D9BUL;
I2C1->TIMINGR = 0x10802D9BUL; // 400 kHz @ 16 MHz
I2C1->CR1 |= I2C_CR1_PE;
I2C1->CR1 |= I2C_CR1_PE;
}
void imu_init()
@@ -68,12 +65,13 @@ 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 |

84
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;
@@ -46,6 +39,9 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
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);
raw.az = (uint16_t) biquad_apply(&accel_z_lpf, raw.az);
@@ -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;
}
////////////////////////////////////////////////////////////
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 = Rev16(raw.ax);
raw.ay = Rev16(raw.ay);
raw.az = Rev16(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 = Rev16(raw.gx);
raw.gy = Rev16(raw.gy);
raw.gz = Rev16(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);
out->ax = raw.ax - accel_bias_x;
out->ay = raw.ay - accel_bias_y;
out->az = raw.az - accel_bias_z;
// калибровка
if (allowed_calib)
imu_calib(raw, 50, 4096, 500);
out->gx = raw.gx - gyro_bias_x;
out->gy = raw.gy - gyro_bias_y;
out->gz = raw.gz - gyro_bias_z;
// масштабирование
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->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;*/
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)
{

16
Source/INS/IRS.c
View File

@@ -11,26 +11,24 @@ void IRS_init(IRS* irs)
void IRS_update(IRS* irs, float dt)
{
Quaternion qConjugate = QuatConjugate(&irs->q);
// gyro update
Vector3 gyro =
{
irs->gyro.x * DEG2RAD,
irs->gyro.y * DEG2RAD,
irs->gyro.z * DEG2RAD
irs->gyro.x * dt * DEG2RAD,
irs->gyro.y * dt * DEG2RAD,
irs->gyro.z * dt * DEG2RAD
};
Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
g = QuatProd(&irs->q, &g);
g = QuatConstProd(&g, dt * 0.5f);
g = QuatConstProd(&g, 0.5f);
irs->q = QuatSum(&irs->q, &g);
irs->q = QuatNormalize(&irs->q, 1.0f);
// /gyro update
// accel update
restoreQuat(irs);
//restoreQuat(irs);
// /accel update
}
@@ -100,14 +98,14 @@ void setAccelShift(IRS* irs, const float roll, const float pitch, const float ya
Vector3 IRS_getGravity(const Quaternion* q)
{
Vector3 g =
Vector3 g =
{
2.0f * (q->x*q->z - q->w*q->y),
2.0f * (q->w*q->x + q->y*q->z),
q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
};
return g;
return g;
}

6
Source/INS/IRS.h
View File

@@ -9,10 +9,10 @@
typedef struct
{
Quaternion q; // ориентация
Quaternion q; // ориентация
Vector3 oriPRT; // orientation pitch roll tilts
Vector3 gyro; // deg/s
Vector3 accel; // g
Vector3 gyro; // deg/s
Vector3 accel; // g
Quaternion shiftAccelPRY; // смещение акселерометра

4
Source/INS/geometry/vector.c
View File

@@ -10,7 +10,7 @@ Vector2 normalizeV2(const Vector2* v, float gain)
Vector3 normalizeV3(const Vector3* v, float gain)
{
Vector3 res = {0};
Vector3 res = {0.0f, 0.0f, 0.0f};
float n = lengthV3(v);
if (n > 1e-12f)
@@ -133,7 +133,7 @@ Vector2 constProdV2(const Vector2* v, float value)
Vector3 constProdV3(const Vector3* v, float value)
{
Vector3 res = {.x = v->x * value, .y = v->y * value, .z = v->z * value};
Vector3 res = {v->x * value, v->y * value, v->z * value};
return res;
}

72
Source/main.c
View File

@@ -10,24 +10,42 @@
void TIM6_DAC_IRQHandler();
static uint8_t irs_update_flag = 0;
//static uint8_t irs_update_flag = 0;
static uint8_t control_update_flag = 0;
static uint8_t allowed_calib = 0;
imu_raw_t imu_raw;
imu_scaled_t imu;
IRS irs;
attitude_t attitude;
rc_channels rx_chs_raw;
rc_channels rx_chs_normalized;
control_channels_t ctrl_chs;
lidar_data lidar;
//lidar_data lidar;
Vector3 euler;
volatile uint32_t us_ticks = 0;
void SysTick_Handler()
{
us_ticks++;
}
float micros()
{
return (float)us_ticks;
}
static float last_time = 0;
void imu_callback(uint8_t* buf);
void delay_ms(uint32_t ms);
int main(void)
{
last_time = micros();
__enable_irq();
NVIC_SetPriority(TIM6_DAC_IRQn, 2);
@@ -40,9 +58,9 @@ int main(void)
imu_init();
imu_processing_init();
//imu_calibrate();
//imu_accel_calibrate();
imu_tim6_init();
imu_tim6_init(500);
IRS_init(&irs);
@@ -54,10 +72,10 @@ int main(void)
while (1)
{
receiver_update(&rx_chs_raw);
rx_chs_normalized = normalize_channels(rx_chs_raw);
receiver_update(&rx_chs_raw);
rx_chs_normalized = normalize_channels(rx_chs_raw);
if (irs_update_flag)
/*if (irs_update_flag)
{
irs_update_flag = 0;
@@ -72,9 +90,9 @@ int main(void)
irs.accel.z = imu.az;
IRS_update(&irs, IMU_DT);
}
}*/
euler = QuatToEuler(&irs.q);
if (control_update_flag)
{
@@ -90,7 +108,7 @@ int main(void)
if (rx_chs_normalized.rc_armed)
{
allowed_calib = 1;
allowed_calib = 0;
motors_set_throttle_mix(
rx_chs_normalized.rc_throttle,
@@ -101,17 +119,47 @@ int main(void)
else
{
motors_turn_off();
allowed_calib = 0;
allowed_calib = 1;
}
}
}
void imu_callback(uint8_t* buf)
{
imu_raw.ax = (buf[0] << 8) | buf[1];
imu_raw.ay = (buf[2] << 8) | buf[3];
imu_raw.az = (buf[4] << 8) | buf[5];
imu_raw.gx = (buf[6] << 8) | buf[7];
imu_raw.gy = (buf[8] << 8) | buf[9];
imu_raw.gz = (buf[10] << 8) | buf[11];
imu_process_raw(&imu, &imu_raw, &allowed_calib);
float now = micros();
float dt = (now - last_time) * 1e-6f;
if (dt <= 0.0f || dt > 0.1f) dt = IMU_DT;
last_time = now;
irs.gyro.x = imu.gx;
irs.gyro.y = imu.gy;
irs.gyro.z = imu.gz;
irs.accel.x = imu.ax;
irs.accel.y = imu.ay;
irs.accel.z = imu.az;
IRS_update(&irs, IMU_DT);
euler = QuatToEuler(&irs.q);
}
void TIM6_DAC_IRQHandler()
{
if (TIM6->SR & TIM_SR_UIF)
{
TIM6->SR &= ~TIM_SR_UIF;
irs_update_flag = 1;
i2c_read_async(ICM_ADDR, 0x2D, 12, imu_callback);
control_update_flag = 1;
}
}