/*
 * msp_TL720D.c
 *
 *  Created on: 2024年1月12日
 *      Author: Administrator
 */

#include "MSP/msp_TL720D.h"
void RF_TL720D_Unscramble();

SP_MSP_RF_TL720D_Parameters *RF_720D;
UART_HandleTypeDef* Huart_RF;
void BCDtoDec(unsigned char BCD_Data[], double *Dec_Data_0);

double Dec_Data;
double Dec_Data_1;

uint8_t RF_TL720D_data[30];

double RF_Roll;

int RF_TL720D_state=0;


uint8_t GF_MSP_RF_TL720D_Init(UART_HandleTypeDef* huart,SP_MSP_RF_TL720D_Parameters *_RF_720D)
{
	Huart_RF=huart;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_RS485_2_RxCpltCallback, RF_TL720D_Unscramble);//这句话的理解有问题
	HAL_UART_Receive_IT(Huart_RF,RF_TL720D_data,1);
	RF_720D = _RF_720D;
	return 1;
}


/**
  * @brief  Tis Function is Ruifen TL720D data decoding
  * @param  Decoding start bit, for example 0x68 0x1f 0x00 0x84 (Four byte frame header，Use 0x68 and 0x1f for verification)，0x00 0x84 For the 0th and 1st positions，Therefore, the data starts in the second place
  * @param  Return data，Euler angle, acceleration, angular velocity
  * @retval
  */
void RF_TL720D_Unscramble()
{
	double RF_data[10];  //double *RF_data[];
	uint8_t Roll_BCD[3];
	int Data_Start=2;
	switch(RF_TL720D_state)
	{
	case 0:
		if(RF_TL720D_data[0]==0x68)
		{
			RF_TL720D_state=1;
			HAL_UART_Receive_IT(Huart_RF,RF_TL720D_data,1);
		}
		else
		{
			RF_TL720D_state=0;
			HAL_UART_Receive_IT(Huart_RF,RF_TL720D_data,1);
		}

		break;
	case 1:
		if (RF_TL720D_data[0]==0x1f)
		{

			HAL_UART_Receive_IT(Huart_RF,RF_TL720D_data,30);
			RF_TL720D_state=2;
		}
		else
		{
			RF_TL720D_state=0;
			HAL_UART_Receive_IT(Huart_RF,RF_TL720D_data,1);
		}

		break;
	case 2:
		RF_TL720D_state=0;
		for(int i=1;i<=9;i++)
		{
			int	Buff_start=Data_Start+(i-1)*3;
			Roll_BCD[0]=RF_TL720D_data[Buff_start];
			Roll_BCD[1]=RF_TL720D_data[Buff_start+1];
			Roll_BCD[2]=RF_TL720D_data[Buff_start+2];
			BCDtoDec( Roll_BCD, &Dec_Data_1);
			RF_data[i-1]=Dec_Data_1*1;
		}

		RF_data[3]=RF_data[3]*0.1;RF_data[4]=RF_data[4]*0.1;RF_data[5]=RF_data[5]*0.1;

		RF_720D->RF_Angle_Roll=RF_data[0]*100;
		RF_720D->RF_Angle_Pitch=RF_data[1];
		RF_720D->RF_Angle_Yaw=RF_data[2];

		RF_720D->RF_Acc_X=RF_data[3];
		RF_720D->RF_Acc_Y=RF_data[4];
		RF_720D->RF_Acc_Z=RF_data[5];

		RF_720D->RF_Gro_X=RF_data[6];
		RF_720D->RF_Gro_Y=RF_data[7];
		RF_720D->RF_Gro_Z=RF_data[8];


//		RF_Roll=RF_data[0];double RF_Pitch=RF_data[1];double RF_Yaw=RF_data[2];
//		double RF_ACC_X=RF_data[3];double RF_ACC_Y=RF_data[4];double RF_ACC_Z=RF_data[5];
//		double RF_Gyr_X=RF_data[6];double RF_Gyr_Y=RF_data[7];double RF_Gyr_Z=RF_data[8];

		HAL_UART_Receive_IT(Huart_RF,RF_TL720D_data,1);
		break;
	}
}
/**
  * @brief  BCD data decoding
  * @param  Input BCD data
  * @param  Output decimal data
  * @retval
  */

void BCDtoDec(unsigned char BCD_Data[], double *Dec_Data_0)
{
	int H_Dec_Data   =(int)(BCD_Data[0]);
	int M_Dec_Data   =(int)(BCD_Data[1]);
	M_Dec_Data= floor(M_Dec_Data/16)*10+(M_Dec_Data%16);
	int L_Dec_Data   =(int)(BCD_Data[2]);
	L_Dec_Data= floor(L_Dec_Data/16)*10+(L_Dec_Data%16);
    if(H_Dec_Data>=10)
    {
    	*Dec_Data_0=-1*((H_Dec_Data-16)*100+M_Dec_Data+L_Dec_Data*0.01);
    }
    else
    {
    	*Dec_Data_0=((H_Dec_Data)*100+M_Dec_Data+L_Dec_Data*0.01);
    }

}
