/*
 * msp_FH_Motor.c
 *
 *  Created on: 2023年12月12日
 *      Author: Administrator
 */
//#include "BHBF_ROBOT.h"
#include "MSP/msp_FH_Motor.h"


uint8_t  Motor_FDCAN_CH_1;
uint8_t FH_Motor_Control_State;
uint16_t FH_Motor_Control_Motor_Index;
FH_MotorParameters*  FH_Motor;
uint8_t FH_Num_Motor=0;

uint8_t Num_Motor_FH=0;
uint16_t  ID_Sent_Offset=(uint16_t)600;
//uint8_t  ID_Recv_Offset=580;


void FH_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data);/*///FrameID, int Length_d,uint8_t *Txdata  //CAN data transmission function*/


void FH_Current_control(uint32_t Motor_ID_1,  int T_Motor_Current_1);
void FH_Velocity_control(uint32_t Motor_ID_1,  int T_Motor_Velocity_1);
void FH_Position_control(uint32_t Motor_ID_1,  int T_Motor_Position_1);
void intToByte_B_FH(int Motor_Velocity,uint8_t *Vel_date);
void Enter_CAN_operation();
void Request_Read_Position(uint32_t Motor_ID_1);
void Request_Read_Velocity(uint32_t Motor_ID_1);
void Request_Read_Current(uint32_t Motor_ID_1);
void GF_MSP_FH_Motor_CANRx_Callback();
void Motor_Fault_Read(uint32_t Motor_ID_1);
void ByteToint_B_FH(uint8_t Vel_buff[],int *Num_data);
void Fault_Anal(uint16_t ID_Value,uint8_t Data_Value[]);
void FH_Motor_Node_SET_1(uint32_t Motor_ID_1);
void FH_Motor_Node_SET_2(uint32_t Motor_ID_1);
void FH_Position_Mode(uint32_t Motor_ID_1);
void FH_Velocity_Mode(uint32_t Motor_ID_1);
void FH_Motor_PowerON(uint32_t Motor_ID_1);
void FH_Motor_PowerOFF(uint32_t Motor_ID_1);


uint8_t GF_MSP_FH_Motor_Init(uint8_t FDCAN_CH, FH_MotorParameters* MotorP,uint8_t Num_Motor_p)
{
	Motor_FDCAN_CH_1=FDCAN_CH;
	FH_Motor_Control_State=0;
	FH_Motor_Control_Motor_Index=ID_Sent_Offset;
	FH_Motor = MotorP;
	FH_Num_Motor = Num_Motor_p;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, GF_MSP_FH_Motor_CANRx_Callback);
	Enter_CAN_operation();
	return 1;
}

uint32_t Node_Offset=700;
void FH_Motor_Node_Prot(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[0];
	int Length_date_1=0;
	FH_FDCANfunction(Motor_ID_1+Node_Offset, Length_date_1,Pos_can_buff_Motor_To);
}

void FH_Motor_Node_SET_1(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2B;
	Pos_can_buff_Motor_To[1]=0x0C;
	Pos_can_buff_Motor_To[2]=0x10;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0xE8;
	Pos_can_buff_Motor_To[5]=0x03;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void FH_Motor_Node_SET_2(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2F;
	Pos_can_buff_Motor_To[1]=0x0D;
	Pos_can_buff_Motor_To[2]=0x10;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x04;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



int FH_Motor_Init_Flag=0;  int FH_Motor_Order=1;
void Motor_Velocity_Enable()
{
	for (FH_Motor_Order=1;FH_Motor_Order<=FH_Num_Motor;FH_Motor_Order++)
	{
		switch(FH_Motor_Init_Flag)
		{
		case 0:
			FH_Motor_Node_SET_1(FH_Motor_Order+FH_Motor_Control_Motor_Index);
			FH_Motor_Init_Flag=FH_Motor_Init_Flag+1;
			break;
		case 1:
			FH_Motor_Node_SET_2(FH_Motor_Order+FH_Motor_Control_Motor_Index);
			FH_Motor_Init_Flag=FH_Motor_Init_Flag+1;
			break;
		case 2:
			FH_Velocity_Mode(FH_Motor_Order+FH_Motor_Control_Motor_Index);
			FH_Motor_Init_Flag=FH_Motor_Init_Flag+1;
			break;
		case 3:
			FH_Motor_PowerON(FH_Motor_Order+FH_Motor_Control_Motor_Index);
			FH_Motor_Init_Flag=FH_Motor_Init_Flag+1;
			break;
		case 4:
			FH_Current_control(FH_Motor_Order+FH_Motor_Control_Motor_Index,  0);
			if(FH_Motor_Order<FH_Num_Motor)
			{
				FH_Motor_Init_Flag=0;
			}
			else
			{
				FH_Motor_Init_Flag=100;
			}
			break;
		default:
	    	break;
		}
	}
}



//开启FH电机CAN通讯 ID 000 数据01 00
void Enter_CAN_operation()
{
	uint32_t Motor_ID_1=000;
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=2;
	Pos_can_buff_Motor_To[0]=0x01;
	Pos_can_buff_Motor_To[1]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



void FH_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	GF_BSP_FDCAN_Senddata(Motor_FDCAN_CH_1,ID,Length,Data);
}


uint32_t FH_Motor_ID_IDD;
void GF_MSP_FH_Motor_Control_Timer()
{
	uint8_t Motor_Run_Mode=FH_Motor[FH_Motor_Control_Motor_Index].Run_Mode;
	uint32_t Motor_ID=FH_Motor[FH_Motor_Control_Motor_Index].MotorID;
	FH_Motor_ID_IDD= Motor_ID;
	switch(FH_Motor_Control_State)
	{
	case 0:
		switch( Motor_Run_Mode )  //butong de moshi
		{
		case DF_MSP_FH_Motor_CurrentMode://
			FH_Current_control(Motor_ID,FH_Motor[FH_Motor_Control_Motor_Index].Target_Current);
			break;
		case DF_MSP_FH_Motor_SpeedMode://
			FH_Velocity_control(Motor_ID,FH_Motor[FH_Motor_Control_Motor_Index].Target_Velcity);
			break;
		case DF_MSP_FH_Motor_PositionMode://
			FH_Position_control(Motor_ID,FH_Motor[FH_Motor_Control_Motor_Index].Target_Position);
			break;
		}
		case 1: //请求读取电机位置
			Request_Read_Position(Motor_ID);
			FH_Motor_Control_State=FH_Motor_Control_State+1;
			break;

		case 2: //请求读取电机速度
			Request_Read_Velocity(Motor_ID);
			FH_Motor_Control_State=FH_Motor_Control_State+1;
			break;

		case 3: //请求读取电机电流
			Request_Read_Current(Motor_ID);
			FH_Motor_Control_Motor_Index++;
			if(FH_Motor_Control_Motor_Index >= FH_Num_Motor)
			{
				FH_Motor_Control_Motor_Index=0;
			}
			FH_Motor_Control_State=0;
			break;
	}
}

void FH_Motor_PowerON(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2B;
	Pos_can_buff_Motor_To[1]=0x40;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x0F;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void FH_Motor_PowerOFF(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2B;
	Pos_can_buff_Motor_To[1]=0x40;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void All_motors_Fault_reset()
{
	uint32_t Motor_ID_1=000;
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=2;
	Pos_can_buff_Motor_To[0]=0x81;
	Pos_can_buff_Motor_To[1]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



void FH_Velocity_control(uint32_t Motor_ID_1,  int T_Motor_Velocity_1)
{
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(T_Motor_Velocity_1, Pos_can_buff_Motor1);
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x23;
	Pos_can_buff_Motor_To[1]=0xFF;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	for(int i=4;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-4];
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void FH_Velocity_Mode(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2F;
	Pos_can_buff_Motor_To[1]=0x60;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;
	Pos_can_buff_Motor_To[4]=0x03;

	for(int i=5;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=00;
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



void FH_Current_control(uint32_t Motor_ID_1,  int T_Motor_Current_1)
{
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(T_Motor_Current_1, Pos_can_buff_Motor1);
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2B;
	Pos_can_buff_Motor_To[1]=0x40;
	Pos_can_buff_Motor_To[2]=0x23;
	Pos_can_buff_Motor_To[3]=0x00;

	for(int i=4;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-4];
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void FH_Position_control(uint32_t Motor_ID_1,  int T_Motor_Position_1)
{
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(T_Motor_Position_1, Pos_can_buff_Motor1);
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x23;
	Pos_can_buff_Motor_To[1]=0x7A;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	for(int i=4;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-4];
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void FH_Position_Mode(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2F;
	Pos_can_buff_Motor_To[1]=0x60;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;
	Pos_can_buff_Motor_To[4]=0x01;

	for(int i=5;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=00;
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}




void intToByte_B_FH(int Motor_Velocity,uint8_t *Vel_date)
{
	uint32_t Motor_Velocity_U= (uint32_t)Motor_Velocity*10;
	//int2byte
	Vel_date[0] =Motor_Velocity_U & 0xff;
	Vel_date[1] =(Motor_Velocity_U & 0xff00)>>8;
	Vel_date[2] =(Motor_Velocity_U & 0xff0000)>>16;
	Vel_date[3] =(Motor_Velocity_U & 0xff000000)>>24;
}


void Request_Read_Position(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x40;
	Pos_can_buff_Motor_To[1]=0x64;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void Request_Read_Velocity(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x40;
	Pos_can_buff_Motor_To[1]=0x69;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Request_Read_Current(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x40;
	Pos_can_buff_Motor_To[1]=0x1C;
	Pos_can_buff_Motor_To[2]=0x22;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Motor_Fault_Read(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x23;
	Pos_can_buff_Motor_To[1]=0x83;
	Pos_can_buff_Motor_To[2]=0x21;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

int Fault_read_FLAG=0;


//位置返回值解析：
//速度返回值解析：
//电流返回值解析；




void GF_MSP_FH_Motor_CANRx_Callback()
{
	uint8_t buf[8];
	uint16_t ID_A_T;
	FDCAN_RxHeaderTypeDef CAN_RX_HDR;
	int Function_code;
	uint8_t Motor_Data[4];
	int Buff_Value_int;
	FDCAN_HandleTypeDef *hfdcan;
	if(Motor_FDCAN_CH_1==1)
	{
		hfdcan = &hfdcan1;
	}
	else
	{
		hfdcan = &hfdcan2;
	}

	if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &CAN_RX_HDR, buf)== HAL_OK)
	{
		ID_A_T=CAN_RX_HDR.Identifier;
		if(ID_A_T>=DF_MSP_FH_Motor_StartID && ID_A_T<DF_MSP_FH_Motor_StartID+Num_Motor_FH)
		{
			FH_Motor[ID_A_T-DF_MSP_FH_Motor_StartID].RxIndex++;
		}
		Function_code=(int)((int32_t)buf[2]<<8|buf[1]);

		for (int i=4;i<8;i++)
		{
			Motor_Data[i-4]=buf[i];
		}

		switch(Function_code)
		{
		case 24676:
			ByteToint_B_FH(Motor_Data, &Buff_Value_int);
			FH_Motor[ID_A_T-DF_MSP_FH_Motor_StartID].Position=  (int32_t)(Buff_Value_int/10);
		break;
		case 24681:
			ByteToint_B_FH(Motor_Data, &Buff_Value_int);
			FH_Motor[ID_A_T-DF_MSP_FH_Motor_StartID].Velcity=  (int32_t)(Buff_Value_int/10);
		break;
		case 8732:
			ByteToint_B_FH(Motor_Data, &Buff_Value_int);
			FH_Motor[ID_A_T-DF_MSP_FH_Motor_StartID].Current=(int32_t)(Buff_Value_int/10);
		break;
		case 8579:
			Fault_Anal(ID_A_T,Motor_Data);
		break;
			}

	}

//	int	ID_A_T=(int)(hfdcan[0]);
//	int Function_code=(int)(hfdcan[1]);
//	uint8_t Motor_Data[4];
//	Motor_Data[0]=hfdcan[2];Motor_Data[1]=hfdcan[3];Motor_Data[2]=hfdcan[4];Motor_Data[3]=hfdcan[5];

}


void Fault_Anal(uint16_t ID_Value,uint8_t Data_Value[])
{
		int32_t Checksum_Sum=(int32_t)((int32_t)Data_Value[3]<<24|Data_Value[2]<<16|Data_Value[1]<<8|Data_Value[0]);

		//0
	switch(Fault_read_FLAG)
	{
	case 0:
	{
		if(((Data_Value[0])&(0X01))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].CRC_Error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].CRC_Error=0;
		}
		//1
		if(((Data_Value[0])&(0X02))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Dri_Inter_Error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Dri_Inter_Error=0;
		}
		//2
		if(((Data_Value[0])&(0X04))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Short_circuit_error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Short_circuit_error=0;
		}
		Fault_read_FLAG=Fault_read_FLAG+1;
		break;
	}

	case 1:
	{
		//3
		if(((Data_Value[0])&(0X08))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Drive_overheating=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Drive_overheating=0;
		}
		//4
		if(((Data_Value[0])&(0X10))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Motor_overheating=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Motor_overheating=0;
		}

		//5
		if(((Data_Value[0])&(0X20))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Overvoltage=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Overvoltage=0;
		}
		Fault_read_FLAG=Fault_read_FLAG+1;
		break;
	}
	case 2:
	{
		//6
		if(((Data_Value[0])&(0X40))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Undervoltage=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Undervoltage=0;
		}
		//7
		if(((Data_Value[0])&(0X80))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Feedback_error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Feedback_error=0;
		}

		//8
		if(((Checksum_Sum)&(0X100))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Phase_error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Phase_error=0;
		}
		Fault_read_FLAG=Fault_read_FLAG+1;
		break;
	}
	case 3:
	{
		//9
		if(((Checksum_Sum)&(0X200))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Follow_error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Follow_error=0;
		}
		//10
		if(((Checksum_Sum)&(0X400))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Overcurrent=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Overcurrent=0;
		}

		//11
		if(((Checksum_Sum)&(0X800))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].FPGA_error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].FPGA_error=0;
		}
		//12
		if(((Checksum_Sum)&(0X1000))>0)
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Input_command_error=1;
		}
		else
		{
			FH_Motor[ID_Value-DF_MSP_FH_Motor_StartID].Input_command_error=0;
		}
		Fault_read_FLAG=0;
	}
 }
}


void ByteToint_B_FH(uint8_t Vel_buff[],int *Num_data)
{
	*Num_data =(int)(Vel_buff[0] | (Vel_buff[1])<<8|(Vel_buff[2])<<16|(Vel_buff[3])<<24);
}









