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

277
Source/BSP/Src/radio_receiver.c
View File

@@ -10,196 +10,159 @@ volatile uint8_t sbus_frame_ready = 0;
void receiver_gpio_init()
{
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
GPIOA->MODER &= ~(3 << (3 * 2));
GPIOA->MODER |= 2 << (3 * 2);
GPIOA->AFR[0] &= ~(0xF << (3 * 4));
GPIOA->AFR[0] |= 12 << (3 * 4);
// pull-up
GPIOA->PUPDR &= ~(3 << (3 * 2));
GPIOA->PUPDR |= 1 << (3 * 2);
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
GPIOA->MODER &= ~(3 << (3 * 2));
GPIOA->MODER |= 2 << (3 * 2);
GPIOA->AFR[0] &= ~(0xF << (3 * 4));
GPIOA->AFR[0] |= 12 << (3 * 4);
// pull-up
GPIOA->PUPDR &= ~(3 << (3 * 2));
GPIOA->PUPDR |= 1 << (3 * 2);
}
void receiver_lpuart_clock_init()
{
RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
}
void receiver_uart_init()
{
receiver_lpuart_clock_init();
LPUART1->CR1 = 0;
LPUART1->CR2 = 0;
LPUART1->CR3 = 0;
LPUART1->BRR = (256 * 16000000UL) / 100000UL;
// parity control enable
LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
// word length M = 01 - 9 bit
LPUART1->CR1 &= ~USART_CR1_M1;
LPUART1->CR1 |= USART_CR1_M0;
// even parity
LPUART1->CR1 &= ~USART_CR1_PS;
// 2 stop bits
LPUART1->CR2 &= ~USART_CR2_STOP;
LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
// invertion enabled
LPUART1->CR2 |= USART_CR2_RXINV;
// receiver enable
// interrupt generated whenever ORE = 1 or RXNE = 1
LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
// uart enable
LPUART1->CR1 |= USART_CR1_UE;
NVIC_EnableIRQ(LPUART1_IRQn);
receiver_lpuart_clock_init();
LPUART1->CR1 = 0;
LPUART1->CR2 = 0;
LPUART1->CR3 = 0;
LPUART1->BRR = (256 * 16000000UL) / 100000UL;
// parity control enable
LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
// word length M = 01 - 9 bit
LPUART1->CR1 &= ~USART_CR1_M1;
LPUART1->CR1 |= USART_CR1_M0;
// even parity
LPUART1->CR1 &= ~USART_CR1_PS;
// 2 stop bits
LPUART1->CR2 &= ~USART_CR2_STOP;
LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
// invertion enabled
LPUART1->CR2 |= USART_CR2_RXINV;
// receiver enable
// interrupt generated whenever ORE = 1 or RXNE = 1
LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
// uart enable
LPUART1->CR1 |= USART_CR1_UE;
NVIC_EnableIRQ(LPUART1_IRQn);
}
void receiver_init()
{
receiver_gpio_init();
receiver_uart_init();
receiver_gpio_init();
receiver_uart_init();
}
void LPUART1_IRQHandler()
{
if (LPUART1->ISR & USART_ISR_RXNE)
{
uint8_t b = LPUART1->RDR;
if (b == SBUS_START_BYTE)
sbus_index = 0;
if (sbus_index < SBUS_FRAME_SIZE)
sbus_buffer[sbus_index++] = b;
if (sbus_index == SBUS_FRAME_SIZE)
{
sbus_index = 0;
sbus_frame_ready = 1;
}
}
if (LPUART1->ISR & USART_ISR_RXNE)
{
uint8_t b = LPUART1->RDR;
if (b == SBUS_START_BYTE)
sbus_index = 0;
if (sbus_index < SBUS_FRAME_SIZE)
sbus_buffer[sbus_index++] = b;
if (sbus_index == SBUS_FRAME_SIZE)
{
sbus_index = 0;
sbus_frame_ready = 1;
}
}
}
void receiver_update(rc_channels* chs)
{
if (!sbus_frame_ready)
return;
sbus_frame_ready = 0;
if (sbus_buffer[0] != SBUS_START_BYTE)
return;
sbus_failsafe = sbus_buffer[23] & (1 << 3);
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
if (sbus_failsafe || sbus_frame_lost)
return;
receiver_parse_frame();
chs->rc_roll = sbus_channels[0];
chs->rc_pitch = sbus_channels[1];
chs->rc_throttle = sbus_channels[2];
chs->rc_armed = sbus_channels[4];
if (!sbus_frame_ready)
return;
sbus_frame_ready = 0;
if (sbus_buffer[0] != SBUS_START_BYTE)
return;
sbus_failsafe = sbus_buffer[23] & (1 << 3);
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
if (sbus_failsafe || sbus_frame_lost)
return;
receiver_parse_frame();
chs->rc_roll = sbus_channels[0];
chs->rc_pitch = sbus_channels[1];
chs->rc_throttle = sbus_channels[2];
chs->rc_armed = sbus_channels[4];
}
void receiver_parse_frame()
{
uint16_t b[22];
for (uint8_t i = 0; i < 22; ++i)
b[i] = sbus_buffer[i + 1];
sbus_channels[0] = ( b[0] | (b[1] << 8) ) & 0x07FF;
sbus_channels[1] = ( (b[1] >> 3) | (b[2] << 5) ) & 0x07FF;
sbus_channels[2] = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) ) & 0x07FF;
sbus_channels[3] = ( (b[4] >> 1) | (b[5] << 7) ) & 0x07FF;
sbus_channels[4] = ( (b[5] >> 4) | (b[6] << 4) ) & 0x07FF;
sbus_channels[5] = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) ) & 0x07FF;
sbus_channels[6] = ( (b[8] >> 2) | (b[9] << 6) ) & 0x07FF;
sbus_channels[7] = ( (b[9] >> 5) | (b[10] << 3) ) & 0x07FF;
sbus_channels[8] = ( b[11] | (b[12] << 8) ) & 0x07FF;
sbus_channels[9] = ( (b[12] >> 3)| (b[13] << 5) ) & 0x07FF;
sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) ) & 0x07FF;
sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) ) & 0x07FF;
sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) ) & 0x07FF;
sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) ) & 0x07FF;
sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) ) & 0x07FF;
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
sbus_failsafe = sbus_buffer[23] & (1 << 3);
uint16_t b[22];
for (uint8_t i = 0; i < 22; ++i)
b[i] = sbus_buffer[i + 1];
sbus_channels[0] = ( b[0] | (b[1] << 8) ) & 0x07FF;
sbus_channels[1] = ( (b[1] >> 3) | (b[2] << 5) ) & 0x07FF;
sbus_channels[2] = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) ) & 0x07FF;
sbus_channels[3] = ( (b[4] >> 1) | (b[5] << 7) ) & 0x07FF;
sbus_channels[4] = ( (b[5] >> 4) | (b[6] << 4) ) & 0x07FF;
sbus_channels[5] = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) ) & 0x07FF;
sbus_channels[6] = ( (b[8] >> 2) | (b[9] << 6) ) & 0x07FF;
sbus_channels[7] = ( (b[9] >> 5) | (b[10] << 3) ) & 0x07FF;
sbus_channels[8] = ( b[11] | (b[12] << 8) ) & 0x07FF;
sbus_channels[9] = ( (b[12] >> 3)| (b[13] << 5) ) & 0x07FF;
sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) ) & 0x07FF;
sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) ) & 0x07FF;
sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) ) & 0x07FF;
sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) ) & 0x07FF;
sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) ) & 0x07FF;
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
sbus_failsafe = sbus_buffer[23] & (1 << 3);
}
rc_channels normalize_channels(rc_channels chs)
{
chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
//chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
return chs;
chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
//chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
return chs;
}
int16_t int_mapping(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
{
return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
}
int8_t bool_mapping_gt(int16_t x, int16_t boundary)
{
return x >= boundary;
}
//------------------------------------------------------------------------------
void toggle_led()
{
if (GPIOA->ODR & (1 << 15))
{
GPIOA->BSRR = 1 << (15 + 16);
}
else
{
GPIOA->BSRR = 1 << 15;
}
}
void led_init(void)
{
/* Enable GPIOA clock */
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
/* PA15 -> Output mode */
GPIOA->MODER &= ~(3U << (15 * 2));
GPIOA->MODER |= (1U << (15 * 2));
/* Push-pull */
GPIOA->OTYPER &= ~(1U << 15);
/* Low speed (?????????? ??? LED) */
GPIOA->OSPEEDR &= ~(3U << (15 * 2));
/* No pull-up / pull-down */
GPIOA->PUPDR &= ~(3U << (15 * 2));
/* Start with LED OFF */
GPIOA->BSRR = (1U << (15 + 16));
}
return x >= boundary;
}