bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Наведён порядок в коде

Browse Source
This commit is contained in:
Radzhab Bisultanov
2026-02-18 14:23:01 +03:00
parent eecf8f2cc2
commit f9b7277a33
20 changed files with 647 additions and 928 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,119 +1,142 @@
#include "imu.h"
void imu_pow_init()
{
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
GPIOC->MODER &= ~(3 << (13 * 2));
GPIOC->MODER |= 1 << (13 * 2);
GPIOC->OTYPER &= ~(1 << 13);
GPIOC->PUPDR &= ~(3 << (13 * 2));
GPIOC->BSRR = 1 << (13 + 16);
}
void i2c_gpio_init()
{
// enable GPIOB clock
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
// Alt function mode PB8, PB9
GPIOB->MODER &= ~((3 << 8 * 2) | (3 << 9 * 2));
GPIOB->MODER |= (2 << (8 * 2)) | (2 << (9 * 2));
// select AF4 for I2C1 at PB8 and PB9
GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
// enable open-drain
GPIOB->OTYPER |= (1 << 8) | (1 << 9);
// set pull-up
GPIOB->PUPDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
GPIOB->PUPDR |= (1 << 8 * 2) | (1 << 9 * 2);
// enable GPIOB clock
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
// Alt function mode PB8, PB9
GPIOB->MODER &= ~((3 << 8 * 2) | (3 << 9 * 2));
GPIOB->MODER |= (2 << (8 * 2)) | (2 << (9 * 2));
// select AF4 for I2C1 at PB8 and PB9
GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
// enable open-drain
GPIOB->OTYPER |= (1 << 8) | (1 << 9);
// set pull-up
GPIOB->PUPDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
GPIOB->PUPDR |= (1 << 8 * 2) | (1 << 9 * 2);
}
void i2c1_init()
{
// enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B;
I2C1->CR1 |= I2C_CR1_PE;
// enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B;
I2C1->CR1 |= I2C_CR1_PE;
}
void icm_init()
void imu_init()
{
// select bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
// wake up, auto clock
i2c_write(ICM_ADDR, REG_PWR_MGMT_1, 1);
// select bank 2
i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
// gyro ~2000 dps, FS_SEL = 3
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, (3 << 1));
// accel ~4g, FS_SEL = 1
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, (1 << 1));
// back to bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
// select bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
// wake up, auto clock
i2c_write(ICM_ADDR, REG_PWR_MGMT_1, 1);
// select bank 2
i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
// gyro ~2000 dps, FS_SEL = 3
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, (3 << 1));
// accel ~4g, FS_SEL = 1
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, (1 << 1));
// back to bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
}
void imu_tim6_init()
{
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
TIM6->PSC = 16000 - 1; // 16 MHz / 16000 = 1000 Hz (1 ms)
TIM6->ARR = 2 - 1; // 2 ms
TIM6->DIER |= TIM_DIER_UIE; // interrupt enable
TIM6->CR1 |= TIM_CR1_CEN; // counter enable
NVIC_EnableIRQ(TIM6_DAC_IRQn);
}
void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
{
i2c_wait_idle(I2C1);
// write register address
I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = reg;
while (!(I2C1->ISR & I2C_ISR_TC));
I2C1->CR2 = (addr << 1) | I2C_CR2_RD_WRN | (len << I2C_CR2_NBYTES_Pos) | I2C_CR2_AUTOEND | I2C_CR2_START;
for (uint8_t i = 0; i < len; ++i)
{
while (!(I2C1->ISR & I2C_ISR_RXNE));
buf[i] = I2C1->RXDR;
}
while (!(I2C1->ISR & I2C_ISR_STOPF));
I2C1->ICR |= I2C_ICR_STOPCF;
i2c_wait_idle(I2C1);
// write register address
I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = reg;
while (!(I2C1->ISR & I2C_ISR_TC));
I2C1->CR2 = (addr << 1) | I2C_CR2_RD_WRN | (len << I2C_CR2_NBYTES_Pos) | I2C_CR2_AUTOEND | I2C_CR2_START;
for (uint8_t i = 0; i < len; ++i)
{
while (!(I2C1->ISR & I2C_ISR_RXNE));
buf[i] = I2C1->RXDR;
}
while (!(I2C1->ISR & I2C_ISR_STOPF));
I2C1->ICR |= I2C_ICR_STOPCF;
}
void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
{
i2c_wait_idle(I2C1);
// write register address
I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = reg;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = data;
while (!(I2C1->ISR & I2C_ISR_TC));
I2C1->CR2 |= I2C_CR2_STOP;
i2c_wait_idle(I2C1);
// write register address
I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = reg;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = data;
while (!(I2C1->ISR & I2C_ISR_TC));
I2C1->CR2 |= I2C_CR2_STOP;
}
void icm_read_raw(imu_raw_t* data)
void imu_read_raw(imu_raw_t* data)
{
uint8_t buf[12];
i2c_read(ICM_ADDR, 0x2D, buf, 12);
data->ax = (buf[0] << 8) | buf[1];
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->gz = (buf[10] << 8) | buf[11];
uint8_t buf[12];
i2c_read(ICM_ADDR, 0x2D, buf, 12);
data->ax = (buf[0] << 8) | buf[1];
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->gz = (buf[10] << 8) | buf[11];
}
static void i2c_wait_idle(I2C_TypeDef* i2c)
{
while (i2c->ISR & I2C_ISR_BUSY);
i2c->ICR = I2C_ICR_STOPCF |
while (i2c->ISR & I2C_ISR_BUSY);
i2c->ICR = I2C_ICR_STOPCF |
I2C_ICR_NACKCF |
I2C_ICR_BERRCF |
I2C_ICR_ARLOCF;