/*
  乐迪r12ds遥控器。
  需要根据实际的遥控器通道修改解析的函数 该遥控器采用sbus  用的uart3

*/


/* Includes ----------------------------------------------------------------- */
#include "r12ds.h"
#include "main.h"
#include "error_detect.h"
#include <string.h>
#include "cmd.h"
#include "user_math.h"

#ifdef r12ds_t
extern UART_HandleTypeDef huart3;
extern DMA_HandleTypeDef hdma_usart3_rx;



osThreadId_t thread_alert;

int buf0ready =0;
int buf1ready =0;

/* Private function  -------------------------------------------------------- */
static void R12DS_IdleCallback(void) {
	static uint16_t this_time_rx_len = 0;
	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);
			
			//1 frame length is correct data
			if(this_time_rx_len == RC_FRAME_LENGTH)
			{
//         	osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF0_REDY);
				buf0ready = 1;
//				detect_hook(R12DS_TOE);
			}
		}
			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)
				{
//           	osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF1_REDY);
					buf1ready = 1;
//					detect_hook(R12DS_TOE);
				}
			}
}

/* Exported functions ------------------------------------------------------- */


int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
{
	    if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
	//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);
	
	//disable dma again but why?
	//what's the condition?
	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);
	
	  BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
                           R12DS_IdleCallback);

  return 1;
}


int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
{
    if (sbus_buf == NULL || rc_ctrl == NULL)
    {
        return 0;
    }
		
		rc_ctrl->ch_x = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1  x
    rc_ctrl->ch_y = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2   mul
    rc_ctrl->mul= ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3  y
                         (sbus_buf[5] << 10)) &0x07ff;
    rc_ctrl->ch_w = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4  w
    
		rc_ctrl->key[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
    rc_ctrl->key[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
    rc_ctrl->key[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
    rc_ctrl->key[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
    rc_ctrl->key[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
    rc_ctrl->key[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
    rc_ctrl->key[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
    rc_ctrl->key[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12

		rc_ctrl->ch_y -= RC_CH_VALUE_OFFSET;
    rc_ctrl->ch_x -= RC_CH_VALUE_OFFSET;
    rc_ctrl->mul  -= RC_CH_VALUE_OFFSET;
    rc_ctrl->ch_w -= RC_CH_VALUE_OFFSET;
    


		rc_ctrl->ch_y -= 4;	//y(-694,693)
		rc_ctrl->ch_x += 3;	//x(-693,694)
		rc_ctrl->mul = rc_ctrl->mul;		//m(-518,843)
		rc_ctrl->ch_w += 4;		//w(-694,693)
	
    rc_ctrl->key[2] = map_fp32(rc_ctrl->key[2],306,1694,1694,306);	
    rc_ctrl->key[3] = map_fp32(rc_ctrl->key[3],306,1694,1694,306);	
		rc_ctrl->ch_x = map_fp32(rc_ctrl->ch_x,700,-800,-700,700);		//x
		rc_ctrl->ch_y = map_fp32(rc_ctrl->ch_y,-900,796,700,-700);	//y
		rc_ctrl->mul = map_fp32(rc_ctrl->mul,-632,901,25,0);		//m
		rc_ctrl->ch_w = map_fp32(rc_ctrl->ch_w,-820,780,-700,700);  
		rc_ctrl->ch_w = 0.5f*(rc_ctrl->ch_w);
    rc_ctrl->ch_x = -0.5f*(rc_ctrl->ch_x);
    rc_ctrl->ch_y = 0.5f*(rc_ctrl->ch_y);		
//		
//̀死区(-5,5)
		if(rc_ctrl->ch_y>-15&&rc_ctrl->ch_y<15) rc_ctrl->ch_y=0;              
		if(rc_ctrl->ch_x>-15&&rc_ctrl->ch_x<15) rc_ctrl->ch_x=0;                
		if(rc_ctrl->mul>=0&&rc_ctrl->mul<=3) rc_ctrl->mul=0;              
		if(rc_ctrl->ch_w>-15&&rc_ctrl->ch_w<15) rc_ctrl->ch_w=0;        

   return 0;
}

#endif