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_ON 1
#define ACCEL_FCHOICE_OFF 0 #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_buf[16];
static uint8_t i2c_len = 0; static uint8_t i2c_len = 0;
static uint8_t i2c_reg = 0; static uint8_t i2c_reg = 0;
static uint8_t i2c_addr = 0;*/ static uint8_t i2c_addr = 0;
typedef struct typedef struct
{ {
@@ -36,7 +36,7 @@ typedef struct
int16_t gx, gy, gz; // lsb int16_t gx, gy, gz; // lsb
} imu_raw_t; } imu_raw_t;
typedef struct I2C_Request /*typedef struct I2C_Request
{ {
uint8_t addr; uint8_t addr;
uint8_t reg; uint8_t reg;
@@ -49,9 +49,9 @@ typedef struct I2C_Request
} I2C_Request; } I2C_Request;
static I2C_Request* i2c_head = 0; 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(); 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_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
void i2c_enqueue(I2C_Request* req); /*void i2c_enqueue(I2C_Request* req);
void i2c_start_next(); void i2c_start_next();*/
void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*)); 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); void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);

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

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

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 // /gyro update
// accel update // accel update
//restoreQuat(irs); // restoreQuat(irs);
// /accel update // /accel update
} }

2
Source/main.c
View File

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