/*
 * msp_MF_Gyroscope.c
 *
 *  Created on: Dec 6, 2024
 *      Author: 13752
 */
#include "MSP/msp_MF_Gyroscope.h"

SP_MSP_Gyro_parameters* Gyro_MFOG40;
UART_HandleTypeDef* Huart_Gyro_MFOG40;

uint8_t Gyro_stas[1];
uint8_t Gyro_Falg = 1;
//uint8_t Gyro_Falg_Head[2];
uint8_t Gyro_Vale_AngularVel[4];
uint8_t Gyro_Vale_Angular[4];
uint8_t Gyro_Vale_Temper[2];
uint8_t Gyro_Vale_Check[2];
uint8_t Gyro_Vale[12];

uint8_t Cheak = 0;


void  GF_Gyro_Unscramble_Callback();
void Gyro_Command_Comp();


uint8_t GF_MSP_Gyro_MFOG40_Init(UART_HandleTypeDef* huart,SP_MSP_Gyro_parameters *_Gyro_MFOG40)
{
	Huart_Gyro_MFOG40 = huart;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_RS485_2_RxCpltCallback, GF_Gyro_Unscramble_Callback);
	HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_stas,1);
	Gyro_MFOG40 = _Gyro_MFOG40;
	return 1;
}

void  GF_Gyro_Unscramble_Callback()
{
	switch(Gyro_Falg)
	{
		case 1:
			if(Gyro_stas[0] == 0x80)
			{
				//Gyro_Falg_Head[0] = Gyro_stas[0];
				HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_stas,1);
				Gyro_Falg = 2;
			}
			else {
				HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_stas,1);
				Gyro_Falg = 1;
			}
		break;
		case 2:
			if(Gyro_stas[0] == 0x80)
			{
				//Gyro_Falg_Head[1] = Gyro_stas[0];
				HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_Vale_AngularVel,4);
				Gyro_Falg = 3;
			}
			else
			{
				HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_stas,1);
				Gyro_Falg = 1;
			}
		break;
		case 3:
			HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_Vale_Angular,4);
			Gyro_Falg = 4;
		break;
		case 4:
			HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_Vale_Temper,2);
			Gyro_Falg = 5;
		break;
		case 5:
			HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_Vale_Check,2);
			Gyro_Falg = 6;
		break;
		case 6:

//			Gyro_Command_Comp();
//			Gyro_Falg = 1;
//			HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_stas,1);

			for(int i=0;i<4;i++)
			{
				Gyro_Vale[i] = Gyro_Vale_AngularVel[i];
			}
			for(int i=0;i<4;i++)
			{
				Gyro_Vale[i+4] = Gyro_Vale_Angular[i];
			}
			for(int i=0;i<2;i++)
			{
				Gyro_Vale[i+8] = Gyro_Vale_Temper[i];
			}
			for(int i=0;i<10;i++)
			{
				Cheak = Cheak + Gyro_Vale[i];
			}

			if(Cheak == Gyro_Vale_Check[1])
			{
				Gyro_Command_Comp();
				Gyro_Falg = 1;
			}
			else
			{
				Gyro_Falg = 1;
			}
			HAL_UART_Receive_IT(Huart_Gyro_MFOG40,Gyro_stas,1);
		break;
	}
}


void Gyro_Command_Comp()
{

    int result = (Gyro_Vale_Angular[0] << 21) + (Gyro_Vale_Angular[1] << 14) + (Gyro_Vale_Angular[2] << 7) + Gyro_Vale_Angular[3];
    double_t result_double = 0;

	switch(result & 0x8000000)
	    {
	        case 0:
	        	result_double = (double)result / 28100;
	            Gyro_MFOG40->Gyro_Angular = result_double;
	        break;
	        case 0x8000000:
	        	result = result - 0xFFFFFFF - 1;
	            result_double = (double)result / 28100;
		         Gyro_MFOG40->Gyro_Angular = result_double;
	        break;
	    }
}


