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-09 14:38:38 +03:00
parent 9948fc6497
commit 941d3c44bb
5 changed files with 72 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,11 +24,11 @@
#define ACCEL_FCHOICE_ON 1
#define ACCEL_FCHOICE_OFF 0
/*static volatile uint8_t i2c_busy = 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;*/
static uint8_t i2c_addr = 0;
typedef struct
{
@@ -36,7 +36,7 @@ typedef struct
int16_t gx, gy, gz; // lsb
} imu_raw_t;
typedef struct I2C_Request
/*typedef struct I2C_Request
{
uint8_t addr;
uint8_t reg;
@@ -49,9 +49,9 @@ typedef struct I2C_Request
} I2C_Request;
static I2C_Request* i2c_head = 0;
static uint8_t i2c_busy = 0;
static uint8_t i2c_busy = 0;*/
//static void (*i2c_callback)(uint8_t* buf) = 0;
static void (*i2c_callback)(uint8_t* buf) = 0;
void imu_pow_init();
@@ -65,8 +65,9 @@ void imu_tim6_init(const uint16_t freq);
void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
void i2c_enqueue(I2C_Request* req);
void i2c_start_next();
/*void i2c_enqueue(I2C_Request* req);
void i2c_start_next();*/
void 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);

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

@@ -1,8 +1,8 @@
#include "imu.h"
static I2C_Request* current_req = 0;
/*static I2C_Request* current_req = 0;
static uint8_t i2c_buf[16];
static uint8_t i2c_index = 0;
static uint8_t i2c_index = 0;*/
void imu_pow_init()
{
@@ -46,13 +46,6 @@ void i2c1_init()
I2C1->TIMINGR = 0x10802D9BUL; // 400 kHz @ 16 MHz
I2C1->CR1 |= I2C_CR1_PE;
I2C1->CR1 |= I2C_CR1_TXIE |
I2C_CR1_RXIE |
I2C_CR1_TCIE |
I2C_CR1_STOPIE;
NVIC_EnableIRQ(I2C1_EV_IRQn);
}
void imu_init()
@@ -114,7 +107,7 @@ void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
I2C1->ICR |= I2C_ICR_STOPCF;
}
void i2c_enqueue(I2C_Request* req)
/*void i2c_enqueue(I2C_Request* req)
{
req->next = 0;
@@ -159,21 +152,30 @@ void i2c_start_next()
I2C1->CR2 = (current_req->addr << 1) |
(1 << I2C_CR2_NBYTES_Pos) |
I2C_CR2_START;
}
}*/
void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*))
{
static I2C_Request req;
if (i2c_busy) return; // ❗ ВАЖНО
static uint8_t buf[16];
i2c_busy = 1;
req.addr = addr;
req.reg = reg;
req.buf = buf;
req.len = len;
req.callback = cb;
i2c_addr = addr;
i2c_reg = reg;
i2c_len = len;
i2c_callback = cb;
i2c_enqueue(&req);
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;
}
void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
@@ -195,47 +197,51 @@ void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
void I2C1_EV_IRQHandler()
{
uint32_t isr = I2C1->ISR;
uint32_t isr = I2C1->ISR;
// --- TXIS ---
if (isr & I2C_ISR_TXIS)
{
I2C1->TXDR = current_req->reg;
}
// TXIS — отправляем регистр
if (isr & I2C_ISR_TXIS)
{
I2C1->TXDR = i2c_reg;
}
// --- TC ---
else if (isr & I2C_ISR_TC)
{
// запускаем чтение
I2C1->CR2 = (current_req->addr << 1) |
I2C_CR2_RD_WRN |
(current_req->len << I2C_CR2_NBYTES_Pos) |
I2C_CR2_AUTOEND |
I2C_CR2_START;
}
// TC — запускаем чтение
else if (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;
}
// --- RXNE ---
else if (isr & I2C_ISR_RXNE)
{
i2c_buf[i2c_index++] = I2C1->RXDR;
}
// RXNE — читаем байты
else if (isr & I2C_ISR_RXNE)
{
static uint8_t index = 0;
i2c_buf[index++] = I2C1->RXDR;
// --- STOP ---
else if (isr & I2C_ISR_STOPF)
{
I2C1->ICR |= I2C_ICR_STOPCF;
if (index >= i2c_len)
index = 0;
}
// копируем данные
for (uint8_t i = 0; i < current_req->len; i++)
current_req->buf[i] = i2c_buf[i];
// STOP — завершение
else if (isr & I2C_ISR_STOPF)
{
I2C1->ICR |= I2C_ICR_STOPCF;
// callback
if (current_req->callback)
current_req->callback(current_req->buf);
i2c_busy = 0;
// следующий запрос
i2c_start_next();
}
I2C1->CR1 |= I2C_CR1_TXIE |
I2C_CR1_RXIE |
I2C_CR1_TCIE |
I2C_CR1_STOPIE;
NVIC_EnableIRQ(I2C1_EV_IRQn);
if (i2c_callback)
i2c_callback(i2c_buf);
}
}
void imu_read_raw(imu_raw_t* data)

4
Source/Core/Src/system_stm32g4xx.c
View File

@@ -283,3 +283,7 @@ void SystemCoreClockUpdate(void)
*/

2
Source/INS/IRS.c
View File

@@ -28,7 +28,7 @@ void IRS_update(IRS* irs, float dt)
// /gyro update
// accel update
//restoreQuat(irs);
// restoreQuat(irs);
// /accel update
}

2
Source/main.c
View File

@@ -115,8 +115,6 @@ void imu_callback(uint8_t* buf)
euler = QuatToEuler(&irs.q);
}
static uint8_t div = 0;
void TIM6_DAC_IRQHandler()
{
if (TIM6->SR & TIM_SR_UIF)