RaDrone/Source/BSP/Src/lidar.c

#include "lidar.h"

volatile uint8_t usart3_rx_buf[USART3_BUF_SIZE];
static uint8_t usart3_rx_head = 0;
static uint8_t usart3_rx_tail = 0;

void lidar_init()
{
	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
	
	// port 11 alt func mode
	GPIOB->MODER &= ~(3 << (11 * 2));
	GPIOB->MODER |= 2 << (11 * 2);
	
	// set AF7 on AFRegister for GPIOB11
	GPIOB->AFR[1] &= ~(0xF << 12);
	GPIOB->AFR[1] |= 7 << 12;
	
	// very high speed
	GPIOB->OSPEEDR |= 3 << (11 * 2);
	
	// pull-up
	GPIOB->PUPDR &= ~(3 << (11 * 2));
	GPIOB->PUPDR |= 1 << (11 * 2);
	
	// SYSCLK selected as USART3 clock
	RCC->CCIPR &= ~(RCC_CCIPR_USART3SEL);
	RCC->CCIPR |= 1 << RCC_CCIPR_USART3SEL_Pos;
	RCC->APB1ENR1 |= RCC_APB1ENR1_USART3EN;
	
	USART3->CR1 = 0;
	USART3->CR2 = 0;
	USART3->CR3 = 0;
	
	USART3->BRR = 16000000UL / 115200UL;
	
	// parity control disable
	USART3->CR1 &= ~USART_CR1_PCE;
	
	// word length 8 bit
	USART3->CR1 &= ~USART_CR1_M1 & ~USART_CR1_M0;
	
	// 1 stop bit
	USART3->CR2 &= ~USART_CR2_STOP;
	
	// receiver enable
	// interrupt generated whenever ORE = 1 or RXNE = 1
	USART3->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
	
	// overrun disable
	// USART3->CR3 |= USART_CR3_OVRDIS;
	
	// USART3 enable
	USART3->CR1 |= USART_CR1_UE;
	
	// Interrupt enable
	NVIC_EnableIRQ(USART3_IRQn);
}

void lidar_tim7_init()
{
	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
	
	TIM7->PSC = 16000 - 1;        // 16 MHz / 16000 = 1000 Hz (1 ms)
	TIM7->ARR = 10 - 1;           // 10 ms
	
	TIM7->DIER |= TIM_DIER_UIE;   // interrupt enable
	TIM7->CR1 |= TIM_CR1_CEN;     // counter enable
	
	NVIC_EnableIRQ(TIM7_DAC_IRQn);
}
	
void TIM7_DAC_IRQHandler()
{
	if (TIM7->SR & TIM_SR_UIF)
	{
		TIM7->SR &= ~TIM_SR_UIF;
		//lidar_update_flag = 1;
	}
}
	
void USART3_IRQHandler()
{
	if (USART3->ISR & USART_ISR_RXNE)
	{
		usart3_rx_buf[usart3_rx_head] = USART3->RDR;
		usart3_rx_head = (usart3_rx_head + 1) % USART3_BUF_SIZE;
	}
}

uint8_t usart_available()
{
	return usart3_rx_head != usart3_rx_tail;
}

uint8_t usart_read()
{
	uint8_t data = usart3_rx_buf[usart3_rx_tail];
	usart3_rx_tail = (usart3_rx_tail + 1) % USART3_BUF_SIZE;
	return data;
}

void lidar_update(lidar_data* lidar)
{
	static uint8_t frame[USART3_FRAME_SIZE];
	static uint8_t index = 0;
	
	while(usart_available())
	{
		uint8_t c = usart_read();
		
		frame[index++] = c;
		
		if (index == USART3_FRAME_SIZE)
		{
			uint8_t checksum = 0;
			for (uint8_t i = 0; i < USART3_FRAME_SIZE - 1; ++i) checksum += frame[i];
			
			if (checksum == frame[USART3_FRAME_SIZE - 1])
			{
				lidar->distance 	= frame[2] | (frame[3] << 8);
                lidar->strength 	= frame[4] | (frame[5] << 8);
                lidar->temperature 	= frame[6] | (frame[7] << 8);
			}
			
			index = 0;
		}
	}
}

void lidar_i2c2_init()
{
	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
	
	GPIOA->MODER &= ~(3 << (8 * 2)) & ~(3 << (9 * 2));
	GPIOA->MODER |= 2 << (8 * 2) | 2 << (9 * 2);		// alt func mode
	
	GPIOA->AFR[1] &= ~(0xF << 0) & ~(0xF << 4);
	GPIOA->AFR[1] |= 4 << 0 | 4 << 4;					// AF4
	
	GPIOA->OTYPER |= 1 << 8 | 1 << 9;					// open-drain
	
	GPIOA->PUPDR &= ~(3 << (8 * 2)) & ~(3 << (9 * 2));
	GPIOA->PUPDR |= 1 << (8 * 2) | 1 << (9 * 2);		// pull-up
	
	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C2EN;				// enable I2C2
	I2C2->TIMINGR = 0x00303D5B;							// 400 kHz @ 16 MHz
	I2C2->CR1 |= I2C_CR1_PE;
}

static void i2c2_wait_txis()
{
    while (!(I2C2->ISR & I2C_ISR_TXIS));
}

static void i2c2_wait_stop()
{
    while (!(I2C2->ISR & I2C_ISR_STOPF));
    I2C2->ICR |= I2C_ICR_STOPCF;
}

static int i2c2_write(uint8_t addr, uint8_t *data, uint8_t size)
{
    while (I2C2->ISR & I2C_ISR_BUSY);

    I2C2->CR2 = 0;
    I2C2->CR2 |= (addr << 1);                 // 7-bit addr
    I2C2->CR2 |= (size << 16);                // bite count
    I2C2->CR2 |= I2C_CR2_AUTOEND;             // auto stop
    I2C2->CR2 |= I2C_CR2_START;               // start

    for (uint8_t i = 0; i < size; i++)
    {
        i2c2_wait_txis();
        I2C2->TXDR = data[i];
    }

    i2c2_wait_stop();

    // check NACK
    if (I2C2->ISR & I2C_ISR_NACKF)
    {
        I2C2->ICR |= I2C_ICR_NACKCF;
        return 0;
    }

    return 1;
}

void tf02_force_uart()
{
    uint8_t cmd_uart[] = {0x5A, 0x05, 0x0A, 0x00, 0x69};
    uint8_t cmd_save[] = {0x5A, 0x04, 0x11, 0x6F};

    // force UART command
    if (!i2c2_write(TF02_I2C_ADDR, cmd_uart, sizeof(cmd_uart)))
    {
        // no ACK — lidar is not on i2c
        return;
    }

    for (volatile int i = 0; i < 100000; i++);

	// save command
    i2c2_write(TF02_I2C_ADDR, cmd_save, sizeof(cmd_save));

    for (volatile int i = 0; i < 200000; i++);
}