RaDrone/Source/BSP/Src/radio_receiver.c

#include "radio_receiver.h"


volatile uint16_t sbus_channels[16] = {0};
volatile uint8_t sbus_buffer[SBUS_FRAME_SIZE] = {0};
volatile uint8_t sbus_index = 0;
volatile uint8_t sbus_failsafe = 0;
volatile uint8_t sbus_frame_lost = 0;
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);
}

void receiver_lpuart_clock_init()
{
	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);
}

void receiver_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;
		}
	}
}

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];
}

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

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

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

int8_t bool_mapping_gt(int16_t x, int16_t boundary)
{
	return x >= boundary;
}