/*
 * 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_1_RxCpltCallback, RF_TL720D_Unscramble);//绑定中断回调函数
	HAL_UART_Receive_IT(Huart_RF,RF_TL720D_data,1);//68
	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++)       //0     1      2  3   4  5   6   7   8
		{						//2+0*3=2,2+1*3=5  8 11  14  17  20 23  26
			int	Buff_start=Data_Start+(i-1)*3;
			Roll_BCD[0]=RF_TL720D_data[Buff_start];//RF_TL720D_data[2]
			Roll_BCD[1]=RF_TL720D_data[Buff_start+1];//RF_TL720D_data[3]
			Roll_BCD[2]=RF_TL720D_data[Buff_start+2];//RF_TL720D_data[4]
			BCDtoDec( Roll_BCD, &Dec_Data_1);
			RF_data[i-1]=Dec_Data_1*1;//roll角存到RF_data[0] 实际值 RF_data[1/2]存另一个角
		}

		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-60;
		//经测试角度偏差值为0.6度，在此修正。
		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);
    }

}
