/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       remote_control.c/h
  * @brief      遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
  *             资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
  *             的方式保证热插拔的稳定性。
  * @note       该任务是通过串口中断启动，不是freeRTOS任务
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-01-2019     RM              1. 完成
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */


#include "ledi.h"
#include "bsp/calc_lib.h"
#include "bsp/uart.h"
#include "device.h"

extern UART_HandleTypeDef huart3;
extern DMA_HandleTypeDef hdma_usart3_rx;

static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl);
void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);

LD_t rc_ctrl;
 uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
uint8_t remote_ready = 0;//ң¿؆÷׼±¸ͪ³ɠ
//´®¿ڤma˫»º³凸³õʼ»¯


void remote_control_init(void)
{
	RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
}

 static uint16_t this_time_rx_len = 0;


void USART3_IRQHandler(void)
{

	//have received data 
	if(huart3.Instance->SR & UART_FLAG_RXNE)
	{
		//ȧ¹ûʇ½ӊՖжϔ򍨹ý¶Áȡdr¼Ĵ憷ǥÁ㍊		__HAL_UART_CLEAR_FEFLAG(&huart3);
	}
	else if(USART3->SR & UART_FLAG_IDLE)
	{
		//ʹӃǥ³ýpe±ꖾλµĺ¯ʽʇҲΪpe idleµȼ¸¸ö֐¶϶¼ʇ¿¿ψ¶Áȡsrԙ¶ÁȡdrǥÁ㵄

		__HAL_UART_CLEAR_PEFLAG(&huart3);
		
		if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
		{
					
			//current memory buffer used is memory0
			
			//disable dma to change dma register
			__HAL_DMA_DISABLE(&hdma_usart3_rx);
			
			//get received data length, length = set_data_length - remain_length 
			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;

			//reset set_data_length
			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
			
			//change memory0 to memory1
			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
			
			//enable dma
			__HAL_DMA_ENABLE(&hdma_usart3_rx);
			
			if(this_time_rx_len ==RC_FRAME_LENGTH)
			{

					sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
				remote_ready = 1;
			}
		}
		
			else
			{
				
				__HAL_DMA_DISABLE(&hdma_usart3_rx);
				
				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
				
				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
				
				//change memory1 to memory0
				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
				
				__HAL_DMA_ENABLE(&hdma_usart3_rx);
				
				if(this_time_rx_len ==RC_FRAME_LENGTH)
				{				
				
					sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
					remote_ready = 1;
				}
			}
	}

}
//void UART3IdleCallback(void) {
//    USART3_IRQHandler();
//}
void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
{
	
	//enable the dma transfer for the receiver request
	SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
	
	//enable idle interrupt
	__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
	
	//disable dma, to change the dma register
	__HAL_DMA_DISABLE(&hdma_usart3_rx);
	
	while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
	{
		__HAL_DMA_DISABLE(&hdma_usart3_rx);
	}
	
	hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
	
	//memory buffer 1
	hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
	
	//momory buffer 2
	hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
	
	//data length
	hdma_usart3_rx.Instance->NDTR = dma_buf_num;
	
	//enable double memory buffer
	SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
	
	
	//enable dma
	__HAL_DMA_ENABLE(&hdma_usart3_rx);

}
bool REMOTE_WaitDmaCplt(uint32_t timeout) {
  return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
          SIGNAL_DR16_RAW_REDY);
}

static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl)
{
    if (sbus_buf == NULL || rc_ctrl == NULL)
    {
        return;
    }
		
	rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1
    rc_ctrl->ch[3] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
    rc_ctrl->ch[1] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3
                         (sbus_buf[5] << 10)) &0x07ff;
    rc_ctrl->ch[2] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
    
	rc_ctrl->sw[6] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
    rc_ctrl->sw[0] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
    rc_ctrl->sw[5] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
    rc_ctrl->sw[4] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
    rc_ctrl->sw[3] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
    rc_ctrl->sw[2] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
    rc_ctrl->sw[1] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
    rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12

	


	rc_ctrl->ch[1] = map_fp32(rc_ctrl->ch[1],352,1695,-1,1);		//lx
	rc_ctrl->ch[0] = map_fp32(rc_ctrl->ch[0],352,1695,-1,1);		//ly
	rc_ctrl->ch[2] = map_fp32(rc_ctrl->ch[2],352,1695,-1,1);		//rx
	rc_ctrl->ch[3] = map_fp32(rc_ctrl->ch[3],352,1695,-1,1);     //ry
	//ˀǸ(-30,30)
	if(rc_ctrl->ch[0]>-0.01&&rc_ctrl->ch[0]<0.01) rc_ctrl->ch[0]=0;              
	if(rc_ctrl->ch[1]>-0.01&&rc_ctrl->ch[1]<=0.01) rc_ctrl->ch[1]=0;                
	if(rc_ctrl->ch[2]>-0.01&&rc_ctrl->ch[2]<=0.01) rc_ctrl->ch[2]=0;              
	if(rc_ctrl->ch[3]>-0.01&&rc_ctrl->ch[3]<=0.01) rc_ctrl->ch[3]=0;        
	//MRobot
	remote_ready = 1;
} 


int8_t LD_HandleOffline( LD_t *ld) {
	if (ld == NULL) return DEVICE_ERR_NULL;  
	memset(&ld, 0, sizeof(LD_t));
	return 0;
	
}

// #include "ledi.h"
// #include "bsp/calc_lib.h"
// #include "bsp/uart.h"

// extern UART_HandleTypeDef huart3;
// extern DMA_HandleTypeDef hdma_usart3_rx;

// static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl);
// void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);

// LD_t rc_ctrl;
//  uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
// uint8_t remote_ready = 0;//遥控器准备完成
// //串口dma双缓冲区初始化


// void remote_control_init(void)
// {
// 	RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
// }

//  static uint16_t this_time_rx_len = 0;


// void USART3_IRQHandler(void)
// {

// 	//have received data 
// 	if(huart3.Instance->SR & UART_FLAG_RXNE)
// 	{
// 		//如果是接收中断则通过读取dr寄存器清零
// 		__HAL_UART_CLEAR_FEFLAG(&huart3);
// 	}
// 	else if(USART3->SR & UART_FLAG_IDLE)
// 	{
// 		//使用清除pe标志位的函数是因为pe idle等几个中断都是靠先读取sr再读取dr清零的
// 		__HAL_UART_CLEAR_PEFLAG(&huart3);
		
// 		if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
// 		{
					
// 			//current memory buffer used is memory0
			
// 			//disable dma to change dma register
// 			__HAL_DMA_DISABLE(&hdma_usart3_rx);
			
// 			//get received data length, length = set_data_length - remain_length 
// 			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;

// 			//reset set_data_length
// 			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
			
// 			//change memory0 to memory1
// 			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
			
// 			//enable dma
// 			__HAL_DMA_ENABLE(&hdma_usart3_rx);
			
// 			if(this_time_rx_len ==RC_FRAME_LENGTH)
// 			{

// 					sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
				
// 			}
// 		}
		
// 			else
// 			{
				
// 				__HAL_DMA_DISABLE(&hdma_usart3_rx);
				
// 				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
				
// 				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
				
// 				//change memory1 to memory0
// 				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
				
// 				__HAL_DMA_ENABLE(&hdma_usart3_rx);
				
// 				if(this_time_rx_len ==RC_FRAME_LENGTH)
// 				{				
				
// 					sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
// 					remote_ready = 1;
// 				}
// 			}
// 	}

// }
// //void UART3IdleCallback(void) {
// //    USART3_IRQHandler();
// //}
// void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
// {
	
// 	//enable the dma transfer for the receiver request
// 	SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
	
// 	//enable idle interrupt
// 	__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
	
// 	//disable dma, to change the dma register
// 	__HAL_DMA_DISABLE(&hdma_usart3_rx);
	
// 	while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
// 	{
// 		__HAL_DMA_DISABLE(&hdma_usart3_rx);
// 	}
	
// 	hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
	
// 	//memory buffer 1
// 	hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
	
// 	//momory buffer 2
// 	hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
	
// 	//data length
// 	hdma_usart3_rx.Instance->NDTR = dma_buf_num;
	
// 	//enable double memory buffer
// 	SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
	
	
// 	//enable dma
// 	__HAL_DMA_ENABLE(&hdma_usart3_rx);

// }

// static void sbus_to_rc(volatile const uint8_t *sbus_buf, LD_t *rc_ctrl)
// {
//     if (sbus_buf == NULL || rc_ctrl == NULL)
//     {
//         return;
//     }
		
// 	rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1
//     rc_ctrl->ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
//     rc_ctrl->ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3
//                          (sbus_buf[5] << 10)) &0x07ff;
//     rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
    
// 	rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
//     rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
//     rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
//     rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
//     rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
//     rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
//     rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
//     rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12

	
// //    rc_ctrl->sw[2] = map(rc_ctrl->sw[2],306,1694,1694,306);	
// //    rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306);	
// //	rc_ctrl->ch[1] = map(rc_ctrl->ch[1],-693,694,-700,700);		//x
// //	rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-694,693,-700,700);		//y
// //	rc_ctrl->ch[2] = map(rc_ctrl->ch[2],200,1800,-700,700);		//x
// //	rc_ctrl->ch[3] = map(rc_ctrl->ch[3],-694,693,-700,700);		//y

// 	rc_ctrl->ch[1] = -map_fp32(rc_ctrl->ch[1],252,1639,-1,1);		//x
// 	rc_ctrl->ch[0] = map_fp32(rc_ctrl->ch[0],306,1693,-1,1);		//y
// 	rc_ctrl->ch[2] = -map_fp32(rc_ctrl->ch[2],74,1422,-1,1);		
// 	rc_ctrl->ch[3] = map_fp32(rc_ctrl->ch[3],194,1580,-1,1);     //x
// 	//死区(-30,30)
// 	if(rc_ctrl->ch[0]>-0.01&&rc_ctrl->ch[0]<0.01) rc_ctrl->ch[0]=0;              
// 	if(rc_ctrl->ch[1]>-0.01&&rc_ctrl->ch[1]<=0.01) rc_ctrl->ch[1]=0;                
// 	if(rc_ctrl->ch[2]>-0.05&&rc_ctrl->ch[2]<=0.05) rc_ctrl->ch[2]=0;              
// 	if(rc_ctrl->ch[3]>-0.01&&rc_ctrl->ch[3]<=0.01) rc_ctrl->ch[3]=0;        
// 	//MRobot
// 	remote_ready = 1;
// }