//********************************************************************************
//								     Include
//********************************************************************************
#include "MDRFxx_SFR.h"
#include "Allhex.h"

//********************************************************************************
//								       RAM
//********************************************************************************
//The variables defined by data are placed in the first 128 bytes (0x00~0x7F) address space
//The variables defined by idata are placed in the (0x00~0xFF) address space
//The variable defined by xdata are placed in the external extended RAM  (generally refers to the external 0xF000~0xF1FF space, the specific space size varies with the MCU , and some MCU do not have external RAM)

//The variable defined by data is the fastest, followed by idata, and xdata is slower than the first two
//Variables that are often used are declared with data/idata; variables that are not commonly used are declared with xdata.
//No special declared variables, the default is data

//MOTOR_COMPONENT xdata Motor_Component;
//PMOTOR_COMPONENT xdata pMotor_Component =& Motor_Component;

unsigned char SystemState = 0;
unsigned char MotorState = 0;
#if (Vbus_Protect == 1)
//unsigned char MotorErrorState = OverVbus | UnderVbus;
unsigned char MotorErrorState = 0;
#else
unsigned char MotorErrorState = 0;
#endif
unsigned char MotorErrorStateOld = 0;
unsigned char MotorFaultState = 0;
unsigned char MotorDir = 0;
bit MotoerStartUpError = 0;

short SpeedCmd = 0;
short SpeedCmdTemp = 0;
short SpeedCmdLimit = SPEED_MAX_LIMIT_VALUE;
short SpeedUiLimit = 0;

short CurrentCmd = 0;
short CurrentCmdTemp = 0;
short CurrentCmdLimit = IQ_MAX_LIMIT_VALUE;

#if ((CONTROL_MODE == Power_Control) || (CONTROL_MODE == Power_Limit))
unsigned short Watt;
unsigned long Watt_C;
#endif

//unsigned short VbusOffset;
unsigned short VbusTemp = 0;
unsigned short Vbus_avg = 0;

unsigned short Heat_Temperture_avg = 1023;
unsigned short Temperture_avg = 0;

unsigned short IbusOffset = 0;
unsigned short IbusTemp = 0;
unsigned short Ibus_avg = 0;

unsigned short VspTemp;
unsigned short Vsp_avg;

xdata short FaultCount = 0;

//xdata unsigned short CloseLoopDelayCount = 0;
xdata unsigned char CloseLoopFlag = 0;
bit POCPCheckFlag = 0;

xdata unsigned char CCWFlag = CW_CCW;
xdata unsigned char CCWFlagOld = 0;

xdata short LatestTheta = 0;
xdata short LatestThetaTemp = 0;

xdata short TailwindSpeed = 0;

xdata unsigned char AngleState = 0;
xdata unsigned char AngleStateOld = 0;
xdata char AngleStateCehckCount = 0;

#if (Vbus_Protect == 1)
xdata unsigned short OverVbusProtectCount = BUS_VOLT_DURATION;
xdata unsigned short UnderVbusProtectCount = BUS_VOLT_DURATION;
#endif
#if (Temperture_Protect == 1)
xdata unsigned short OverTempertureProtectCount = TEMPERTURE_DURATION;
#endif
#if (Heat_Temperture_Protect == 1)
xdata unsigned short OverHeatTempertureProtectCount = HEAT_TEMPERTURE_DURATION;
#endif
#if (POCP_Protect == 1)
xdata unsigned short POCP_ProtectCount = PHASE_OCP_DURATION;
//xdata unsigned short POCP_ProtectCount1;
//xdata unsigned short POCP_ProtectCount2;
//xdata unsigned short POCP_ProtectCount3;
#endif
#if (MotorLackPhase_Run == 1)
xdata unsigned short MotorLackPhase_Run_ProtectCount;
xdata unsigned short ProtectCount1;
xdata unsigned short ProtectCount2;
xdata unsigned short ProtectCount3;
#endif
xdata unsigned short BreakCount = 0;
xdata unsigned short BreakPwmDuty = 0;

xdata unsigned short StopCheckCount = 0;
xdata unsigned short AlignmentCount = 0;
xdata unsigned short RampDelayCount = 0;
xdata unsigned short TailwindCount = 0;
xdata unsigned short TailwindTimeOut = 0;
xdata unsigned char MotorStartRetryCount = 0;
xdata unsigned short MotorStartDelayCount1 = 0;
xdata unsigned short MotorStartDelayCount2 = 0;
xdata unsigned short BumplessCount = 0;

xdata unsigned short MotorFaultDelayCount = 0;

xdata int Speed;
xdata short EstimatedSpeed;
xdata long EstimatedSpeed_C;

xdata short IaFb;
xdata short IbFb;
xdata short IcFb;
xdata short IdFb;
xdata short IqFb;
xdata short IqPiOut;

xdata short BmfVW_Sub;
xdata short BmfVW_Sum_Pos_ZeroPoint;
xdata short BmfVW_Sum_Neg_ZeroPoint;
xdata short BmfU;
xdata short BmfV;
xdata short BmfW;

xdata char ZeroPointDebounceCnt;
xdata char ZeroPointCnt;
xdata char ZeroPointCheck = 0;
xdata char ZeroPointFlag;
xdata char ZeroPointFlagOld;

xdata char Flag_BEMF_Rising_Edge  = 0;
xdata char Flag_BEMF_Falling_Edge = 0;

xdata unsigned short BEMF_SPEED = 0;
xdata unsigned char BemfStateOld = 0;
xdata unsigned char BemfState = 0;

//xdata unsigned short HallUEdgeCnt = PWM_Frequency;
//xdata unsigned short RecHallUEdgeCnt = PWM_Frequency;
xdata unsigned short HallUEdgeCnt = 0;
xdata unsigned short RecHallUEdgeCnt = 0;

xdata short PllKpTemp = 0;
xdata short PllOutTemp = 0;
xdata short PllOutTempSpeed = 0;
xdata short PllCount = 0;

xdata short IqCmdTemp = 0;
xdata short IdCmdTemp = 0;

//xdata unsigned short BmfBreakCount = 0;

xdata unsigned short Ramp_Cnt = 0;
xdata unsigned short VspDelayCount = 0;

bit SlowSpeedFlag = 0;
#if (Vsp_LookUpTable == 1)
code const ConstP rtConstP = LOOKUPTABLE_DEFAULTS;
#endif
#if (Power_LookUpTable == 1)
code const ConstP rtConstP_P = LOOKUPTABLE_DEFAULTS_P;
#endif

xdata signed long IaFb_C,IbFb_C,IcFb_C;//IdFb_C;
#if ((MotorLackPhase == 1) || (MotorLackPhase_Run == 1))
xdata unsigned short LackPhaseCount = 0;
xdata unsigned char LackPhaseState = 0;
#endif

#if (PhaseLoss_Fun == 1)
unsigned int fil(unsigned int a)
{
	static unsigned long b;
	static unsigned int c,d;
	if(c ++ > 128)
	{
		d = b >> 7;
		c = 0;
		b = 0;
	}
	else
	{
		b = b + a;
	}
	return d;
}

signed int Abs_ia,Abs_ib,Abs_ic,IQ_FB_V,IQ_FB_F;
unsigned int Max_ia,Max_ib,Max_ic;
signed int LowCurrent,NomCurrent,Currentoffset;
unsigned int AOpencnt,BOpencnt,COpencnt;
unsigned char LphaseDelayCount,Lphasecnt,mcLossPHTimes;
void PhaseLoss(void)
{
	//signed int Tmp;
//		SFR_PAGE = 2; Tmp = FOC_D1;
//		Abs_ia = DATA_FILTER(Tmp, Abs_ia_C, Abs_ia, signed short, 8);
//		SFR_PAGE = 2; Tmp = FOC_D2;
//		Abs_ib = DATA_FILTER(Tmp, Abs_ib_C, Abs_ib, signed short, 8);
	if((MotorState == M_START) || (MotorState == M_RUN)){
		SFR_PAGE = 2;
		Abs_ia = FOC_D1;
		Abs_ib = FOC_D2;
		Abs_ia = abs(Abs_ia);
		Abs_ib = abs(Abs_ib);
		Abs_ic = abs(-Abs_ia-Abs_ib);
		SFR_PAGE = 0; IQ_FB_V = FOC_D1;
		SFR_PAGE = 0;IQ_FB_F = fil(abs(IQ_FB_V));
		LowCurrent = 300;
		NomCurrent = IQ_FB_F;
		Currentoffset = (int)(float)(IQ_FB_F*3);
		//mcLossPHcnts++;
		LphaseDelayCount++;
		if(LphaseDelayCount >= 10 && Error_Status == 0){ // Period 10ms
		//if(LphaseDelayCount >= 10 && MotorErrorState == 0){
			LphaseDelayCount = 0;
			if(Abs_ia > Max_ia) Max_ia = Abs_ia;
			if(Abs_ib > Max_ib) Max_ib = Abs_ib;
			if(Abs_ic > Max_ic) Max_ic = Abs_ic;
		
			Lphasecnt++;
			if(Lphasecnt > 2){ // Period 20ms
				Lphasecnt = 0;
				//if((mcLossPHcnts > 100) && (mcLossPHcnts <= 700)){ // StartUp 
				if(MotorState == M_START){ 
					if(Max_ia < 200 && Max_ib > 6000 && Max_ic > 6000)   AOpencnt++;
					else                                                 AOpencnt = 0;
					if(Max_ia > 6000 && Max_ib < 200 && Max_ic > 6000)   BOpencnt++;
					else                                                 BOpencnt = 0;
					if(Max_ia > 6000 && Max_ib > 6000 && Max_ic > 12000) COpencnt++;
					else                                                 COpencnt = 0;
					if(Max_ia < 200 && Max_ib < 200 && Max_ic < 350) mcLossPHTimes++;
					else                                             mcLossPHTimes = 0;
					Max_ia = 0;
					Max_ib = 0;
					Max_ic = 0;
					if((AOpencnt > 0) || (BOpencnt > 0) || (COpencnt > 0)|| (mcLossPHTimes > 0)){
						mcLossPHTimes = 10;
						MotorErrorState |= LackPhase;
					}
				}
				//else if(mcLossPHcnts > 700){ // StartRun
				else if(MotorState == M_RUN){
					if(Max_ia < LowCurrent && Max_ib > NomCurrent && Max_ic > NomCurrent)    AOpencnt++;
					else                                                                     AOpencnt = 0;
					if(Max_ib < LowCurrent && Max_ia > NomCurrent && Max_ic > NomCurrent)    BOpencnt++;
					else                                                                     BOpencnt = 0;
					if(Max_ic > Currentoffset && Max_ib > NomCurrent && Max_ia > NomCurrent) COpencnt++;
					else                                                                     COpencnt = 0;
					if(Max_ia < 200 && Max_ib < 200 && Max_ic < 350) mcLossPHTimes++;
					else                                             mcLossPHTimes = 0;
					Max_ia = 0;
					Max_ib = 0;
					Max_ic = 0;
					if((AOpencnt > 2) || (BOpencnt > 2) || (COpencnt > 2) || (mcLossPHTimes > 5)){
						mcLossPHTimes = 10;
						MotorErrorState |= LackPhaseRun;
					}
				}
			}
		}
	}
}
#endif
#if (MotorLackPhase == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void MotorLackPhase_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void MotorLackPhase_Fun (void)
{
	//static idata signed long IaFb_C,IbFb_C;
	signed int Tmp,Tmp1;
//	IQ_CMD_MACRO(MotorLackPhase_Detect_IQ);
//	IQ_KP_MACRO(Iq_Kp);
//	IQ_KI_MACRO(Iq_Ki);
	VQ_OFFSET_MACRO(Detect_Vq);
	//MOTOR_CONT1 |= FOCANGSEL;// Use SMO_Ang
	LatestTheta = 160<<6; // Adjust SIN and let W Input, U & V Output for current direction
	FOC_ANG_MACRO(LatestTheta);
	//CPU_ANG_MACRO(LatestTheta>>6);
	MOTOR_CONT1 |= (MPWMSEL); // Enable PWM Smo Mode
	#if (MotorLackPhase_SOP == 1)
	PWM_SetActive();
	#endif
	SFR_PAGE = 2; Tmp = abs(FOC_D1); // abs(Ia)
	SFR_PAGE = 2; Tmp1 = abs(FOC_D2); // abs(Ib)
	IaFb = DATA_FILTER(Tmp, IaFb_C, IaFb, signed short, 3);
	IbFb = DATA_FILTER(Tmp1, IbFb_C, IbFb, signed short, 3);
	
	if(  (IaFb < MotorLackPhase_Threshold_0)
		|| (IbFb < MotorLackPhase_Threshold_1))
	{
		LackPhaseState = 1;// 1:Lackphase  0:normal
		LackPhaseCount++;
		if(LackPhaseCount >= LackPhase_Duration){
			LackPhaseCount = 0;
			MotorErrorState |= LackPhase;
			VQ_OFFSET_MACRO(0);
		}
	}
	else{
		LackPhaseState = 0;
		MotorErrorState &= ~(LackPhase);
		#if (MotorLackPhase_SOP == 2)
			LackPhaseCount = 0;
			IaFb = 0;
			IbFb = 0;
			IaFb_C = 0;
			IbFb_C = 0;
//			IQ_CMD_MACRO(0);
//			IQ_KP_MACRO(0);
//			IQ_KI_MACRO(0);
			VQ_OFFSET_MACRO(0);
			MotorState = M_IPD;
		#endif
	}
}
#endif 

#if (MotorLackPhase_Run == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void MotorLackPhase_Run_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void MotorLackPhase_Run_Fun (void)
{
	signed int Tmp1,Tmp2,Tmp3;
//	signed int Tmp;
	if((MotorState == M_RUN)
		//||(MotorState == M_START)
	){
		SFR_PAGE = 2; Tmp1 = abs(FOC_D1); // abs(Ia) 
		SFR_PAGE = 2; Tmp2 = abs(FOC_D2); // abs(Ib) 
		//  IcFb means  IqFb should multiple  GAIN to normalize  , should  IcFB > IaFB and IcFB > IbFB
		//SFR_PAGE = 0; Tmp3 = abs(FOC_D1); // abs(IqFB)
		//Tmp3 = (Tmp3 * 2)/3;
		//Tmp3 = (signed int)((float) Tmp3 * IqFb_Gain); 
		
		// detect abs(Ia-Ib). If C phase is break, Ia,Ib will have current direction. abs(Ia-Ib) = 0 and it means lack phase. 
		Tmp3 = abs(Tmp1 - Tmp2);
		
		IaFb = DATA_FILTER(Tmp1, IaFb_C, IaFb, signed short, 3);
		IbFb = DATA_FILTER(Tmp2, IbFb_C, IbFb, signed short, 3);
		IcFb = DATA_FILTER(Tmp3, IcFb_C, IcFb, signed short, 3);
		if(  (IaFb < MotorLackPhase_Run_Threshold_0)
			|| (IbFb < MotorLackPhase_Run_Threshold_1)
			|| (IcFb < MotorLackPhase_Run_Threshold_2))
		{
			if(LackPhaseCount >= LackPhase_Run_Duration)
			{
				LackPhaseCount = LackPhase_Run_Duration;
				LackPhaseState = 1;// 1:Lack phase 0:Normal
				MotorErrorState |= LackPhaseRun;
			}
			else LackPhaseCount += Timer0_1ms;
		}
		else
		{
			if(LackPhaseCount < Timer0_1ms)
			{
				LackPhaseCount = 0;
				LackPhaseState = 0;
				MotorErrorState &= ~(LackPhaseRun);
			}
			else
				LackPhaseCount -= Timer0_1ms;
		}
	}
}
#endif
#if (VSP_TRI == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void Vsp_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void Vsp_Fun (void)
{
	if(Vsp_avg >= 102) // 204 = 1V
	{
		SystemState = 1;
		#if (Vsp_LookUpTable == 1)
			#if (CONTROL_MODE == Current_Control)
			CurrentCmd = look1_binlx(Vsp_avg, rtConstP.uDLookupTable_bp01Data, rtConstP.uDLookupTable_tableData, 5);
			#endif
			#if (CONTROL_MODE == Speed_Control) || (CONTROL_MODE == Power_Limit)
			SpeedCmd = look1_binlx(Vsp_avg, rtConstP.uDLookupTable_bp01Data, rtConstP.uDLookupTable_tableData, 5);
			#endif
		#else
			#if (CONTROL_MODE == Current_Control)
			CurrentCmd = (short) ((float) Vsp_avg / 1023 * IQ_MAX_LIMIT_VALUE);
			#endif
			#if (CONTROL_MODE == Speed_Control) || (CONTROL_MODE == Power_Limit)
			SpeedCmd = (short) ((float) Vsp_avg / 1023 * SPEED_MAX_LIMIT_VALUE);
			#endif
		#endif
	}
	else
	{
		if(Vsp_avg < 51)
		{
			#if (CONTROL_MODE == Current_Control)
			CurrentCmd -= (0.001*I_AMPLIFIER);
			if(CurrentCmd < (0.1*I_AMPLIFIER))
			#endif
			#if (CONTROL_MODE == Speed_Control)
			SpeedCmd -= 1;
			if(SpeedCmd < (0.02*SPEED_MAX_LIMIT_VALUE))
			#endif
			{
				SystemState = 0;
				MotorErrorState = 0;
				CurrentCmd = 0;
				SpeedCmd = 0;
			}
		}
	}
}
#endif
/*---------------------------------------------------------------------------*/
/* Name        : void Motor_Control (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void Motor_Control (void)
{
	if(CCWFlag != CCWFlagOld)
		MotorState = M_INIT;
	CCWFlagOld = CCWFlag;
	
	if((MotorErrorState) // Motor Error
		|| (HeatErrorState)
		//|| (ZERO_Check==1)
	)
	{
		//if(MotorState != M_OFF) 
                ResetMOC();
		//PWM_SetAllOff();
		if(SW_HOT_COOL) SW_HOT_COOL = 0;
		Brake_Motor();
		MotorStartRetry_Flow();
		MotorErrorState = Clear;
		//TR2 = 0; 
		//P0_4 = 0; 
	}
	else if(SystemState
                //&&(ZERO_Check == 0)
        ) // Motor Start
	{
		MotorStart_Flow();
		//TR2 = 1;
	}
	else // Motor Stop
	{
		//if(MotorState != M_OFF) 
                        ResetMOC();
		//PWM_SetAllOff();
		if(SW_HOT_COOL) SW_HOT_COOL = 0;
		Brake_Motor();
		MotorStartRetryCount = 0;
		MotorErrorState = Clear;
		MotorState = M_OFF;
		//TR2 = 0;
		//P0_4 = 0;
	}
}
/*---------------------------------------------------------------------------*/
/* Name        : void MotorStartRetry_Flow (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void MotorStartRetry_Flow (void)
{
	MotorErrorStateOld = MotorErrorState;
	if(
		#if (AOCP_Retry_ENABLE == 1)
			((MotorErrorState & AOCP) == AOCP)||
		#endif
		#if (POCP_Retry_ENABLE == 1)
			((MotorErrorState & POCP) == POCP)||
		#endif
		#if (FaultLock_Retry_ENABLE == 1)
			((MotorErrorState & FaultLock) == FaultLock)||
		#endif
		#if (MotorLackPhase_Retry_ENABLE == 1)
			((MotorErrorState & LackPhase) == LackPhase)||
		#endif
		#if (MotorLackPhase_Run_Retry_ENABLE == 1)
			((MotorErrorState & LackPhaseRun) == LackPhaseRun)||
		#endif
			(MotoerStartUpError)|| // start up error
		0)
	{
		MotorStartDelayCount1++;
		if(MotorStartDelayCount1 >= RESTART_DURATION)
		{
			MotorStartDelayCount1 = 0;
			MotorStartRetryCount++;
			if(MotorStartRetryCount >= RETRY_COUNT)
			{
				MotorState = M_ERROR;
			}
			else
			{
				if((MotorErrorState & AOCP) == AOCP)
					OCPCONT |= 0x02;// Clear AOCP / DOCP Status
				MotorState = M_OFF;
				MotorErrorState = Clear;
				if(MotoerStartUpError) MotoerStartUpError = 0;
			}
		}
	}
	else
	{
		MotorState = M_ERROR;
		//MotorState = M_INIT;
	}
}
/*---------------------------------------------------------------------------*/
/* Name        : void MotorStart_Flow (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void MotorStart_Flow (void)
{
	signed int Tmp;
	switch (MotorState)
	{
		case M_INIT:
			PWM_SetAllOff();
			ResetMOC();
			MotorInit_Fun();
			if(CCWFlag) MOTOR_CONT2 |= 0x08;
			else        MOTOR_CONT2 &= ~(0x08);
			MotorState = M_TAILWIND;
			break;
		case M_BMF_BREAK:
			PWM_SetBreakForce();
			SFR_PAGE = 2; IaFb = abs(FOC_D1); // abs(Ia)
			//IaFb = DATA_FILTER(Tmp, IaFb_C, IaFb, signed short, 8);
			SFR_PAGE = 2; IbFb = abs(FOC_D2); // abs(Ib)
			//IbFb = DATA_FILTER(Tmp, IbFb_C, IbFb, signed short, 8);
			if(((IaFb) < BREAK_PHASE_CURRENT_VALUE) && ((IbFb) < BREAK_PHASE_CURRENT_VALUE))
			{
				BreakCount++;
				if(BreakCount >= BREAK_DURATION){
					BreakCount = 0;
					SlowSpeedFlag = 0;
					MotorDir = 0;
					IaFb = 0;
					IbFb = 0;
					IaFb_C = 0;
					IbFb_C = 0;
					PWM_SetAllOff();
					#if (BEMF_TWO_TAILWIND_FUNCTION_RES == 1) || (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 1)
						MotorState = M_TAILWIND; // To M_TAILWIND
					#else
						MotorState = M_IPD; // To M_IPD
					#endif
				}
			}
			else
				BreakCount = 0;
			break;
		case M_TAILWIND:
			#if (BEMF_TWO_TAILWIND_FUNCTION_RES == 1) || (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 1)
				#if (BEMF_TAILWIND_SOP == 2)
					if(TailwindTimeOut > (TAILWIND_DURATION))
					{
						TailwindTimeOut = 0;
						MotorDir = 0;
						#if (MotorLackPhase == 1)
							MotorState = M_LACK_PHASE;
						#else
							MotorState = M_IPD;
						#endif
					}
					else
						TailwindTimeOut++;
				#endif
			#endif
			#if (BEMF_ONE_TAILWIND_FUNCTION == 1)
				#if (BEMF_TAILWIND_SOP == 2)
					if(TailwindTimeOut > (TAILWIND_DURATION))
					{
						TailwindTimeOut = 0;
						MotorDir = 0;
						#if (MotorLackPhase == 1)
							MotorState = M_LACK_PHASE;
						#else
							MotorState = M_IPD;
						#endif
					}
					else
						TailwindTimeOut++;
				#endif
			#endif
			#if ((BEMF_TWO_TAILWIND_FUNCTION_RES == 0) && (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 0)  && (BEMF_ONE_TAILWIND_FUNCTION == 0))
				#if (MotorLackPhase == 1)
					MotorState = M_LACK_PHASE;
				#else
					// MotorState = M_IPD;
					MotorState = M_BMF_BREAK;
				#endif
			#endif
			break;
		#if (MotorLackPhase == 1)
		case M_LACK_PHASE:
			MotorLackPhase_Fun();
			break;
		#endif
		case M_IPD:
			#if (IPD_FUNCTION == 1)
				IPDDetect_Fun();
			#else 
				#if (SLOW_SPEED == 1)
					//=================================================
					// TailWind Slow Speed Break
					//=================================================
					if((MotorDir == 0) && (SlowSpeedFlag == 1)) // Motor start to run but MCU still does know the direction 
					{
						// To M_BREAK
						MotorState = M_BMF_BREAK;
					}
					else
					{
						// To M_START
						MotorState = M_START;
						LatestTheta = 160<<6;
					}
				#else
					MotorState = M_START;
					LatestTheta = 160<<6;
				#endif
			#endif
			break;
		case M_START:
			if(MotorDir == 0)
				IPDStart_Fun();
			else
				TailWindStart_Fun();
			MotorCloseLoop_Fun();
			break;
		case M_RUN:
			//TR2 = 1; // FG Enable
			#if (CONTROL_MODE == Current_Control)
					//CurrentCmdTemp = Ramp_Fun(CurrentCmd,CurrentCmdTemp,IQ_MAX_LIMIT_VALUE,0,1);
					CurrentCmdTemp = CurrentCmd;
					//if(CurrentCmdTemp >= IQ_MAX_LIMIT_VALUE) CurrentCmdTemp = IQ_MAX_LIMIT_VALUE;
					#if (Temperture_Protect == 1)
						IQ_CMD_MACRO(CurrentCmdTemp-Temperture_I);
					#else
						IQ_CMD_MACRO(CurrentCmdTemp);
					#endif
			#endif
			#if (CONTROL_MODE == Speed_Control)
					SpeedCmdTemp = Ramp_Fun(SpeedCmd,SpeedCmdTemp,SPEED_MAX_LIMIT_VALUE,0,20);
					//SpeedCmdTemp = SpeedCmd; if(SpeedCmdTemp >= SPEED_MAX_LIMIT_VALUE) SpeedCmdTemp = SPEED_MAX_LIMIT_VALUE;
					SPEED_CMD_MACRO(SpeedCmdTemp);
					#if (Temperture_Protect == 1)
						SPEED_MAX_MACRO(Spd_MaxLimit-Temperture_I);
					#endif
			#endif
			#if (CONTROL_MODE == Power_Limit)
					PowerLimit_Fun();
			#endif
			#if (CONTROL_MODE == Power_Control)
					PowerControl_Fun();
			#endif
					RampDelayCount++;
					if(RampDelayCount >= 10)
					{
						RampDelayCount = 0;
						// ======== TailWind Clear VQ_OFFSET ========
						SFR_PAGE = 0; Tmp = VDQ_OFST; SYNC = 0x55;
						if(Tmp >= 10){
							Tmp -= 10;
							SFR_PAGE = 0; VDQ_OFST = Tmp; SYNC = 0x55;
						}
						// ===============================
						IdCmdTemp = Ramp_Fun(ID_FINAL_START_VALUE,IdCmdTemp,32767,-32767,16);
						ID_CMD_MACRO(IdCmdTemp);
					}
				//===================================
				// Motor Stop Fun
				//===================================
				if(EstimatedSpeed < STOP_SPEED_VALUE)
				{
					StopCheckCount++;
					if(StopCheckCount > STOP_SPEED_DURATION)
					{
						MotoerStartUpError = 1;
						StopCheckCount = 0;
						SystemState = 0;
						ResetMOC();
						MotorState = M_OFF;
					}
				}
				else
					StopCheckCount = 0;
			break;
		default:
			ResetMOC();
			PWM_SetAllOff();
			MotorState = M_INIT;
			break;
	}
}
/*---------------------------------------------------------------------------*/
/* Name        : void MotorInit_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void MotorInit_Fun (void)
{
	#if (CONTROL_MODE == Power_Control) 
	InitPI(&PIParm_Watt);
	#endif
	//Iq_PI_Control_Init();
	SFR_PAGE = 0;
	//PI_CMD = 0;
	PI_KP = 0;
	PI_KI = 0;
	//PI_UI = 0;
	PI_LMT = Iq_MaxLimit;
	
	//Id_PI_Control_Init();
	SFR_PAGE = 1;
	//PI_CMD = 0;
	PI_KP = 0;
	PI_KI = 0;
	//PI_UI = 0;
	PI_LMT = Id_MaxLimit;
	
	//Speed_PI_Control_Init();
	SP_CYC = Spd_Cyc;// calculate closeloop after xx times PWM cycles. 
	SFR_PAGE = 2;
	//PI_CMD = 0;
	PI_KP = 0;
	PI_KI = 0;
	//PI_UI = 0;
	PI_LMT = Spd_MaxLimit;
	
	//PLL_PI_Control_Init();
	SFR_PAGE = 3;
	//PI_CMD = 0;
	PI_KP = 0;//Pll_Kp_TailWind;
	PI_KI = 0;//Pll_Ki;
	//PI_UI = 0;
	PI_LMT = Pll_MaxLimit;
	
	VQ_OFFSET_MACRO(0);
	//VD_OFFSET_MACRO(0);
	
	GS_MACRO(G);
	SMO_KSLIDE_MACRO(Kslide);
	ANG_BASE_MACRO(AngleBase);

	FS_MACRO(F);
	SMO_FILTER_MACRO(Kslf);
	ANG_SHIFT_MACRO(0);
	
	SVPWM_AMP_MACRO(SVPWM_AMP_REGS); 
	
	FOCCONT |= (PLLEN);
	MOTOR_CONT1 |= (FOCANGSEL);// Enable SMO_Ang

	FOCCONT &= ~(SPEEDEN);
	MOTOR_CONT1 &= ~(IQINSEL);// Disable SPEED_PI_OUT
	MOTOR_CONT2 &= ~(0x10);

	SpeedCmd = SPEED_MAX_LIMIT_VALUE;
	SpeedCmdTemp = FREQ_FINAL_START_VALUE;

	CurrentCmd = IQ_MAX_LIMIT_VALUE;
	CurrentCmdTemp = IQ_FINAL_START_VALUE;;
	
	IqCmdTemp = 0;
	IdCmdTemp = 0;
	
	MotorDir = 0;
	ZeroPointCheck = 0;
	ZeroPointFlag = 0;
	BmfVW_Sub = 0;
	RecHallUEdgeCnt = PWM_Frequency;
	HallUEdgeCnt = 1;
	ZeroPointCnt = 0;
	ZeroPointDebounceCnt = 0;
	BmfVW_Sum_Pos_ZeroPoint = 0;
	BmfVW_Sum_Neg_ZeroPoint = 0;
	SlowSpeedFlag = 0;
	
	IaFb = 0;
	IbFb = 0;
	IaFb_C = 0;
	IbFb_C = 0;
	
	LatestTheta = 0;
	RampDelayCount = 0;
	AngleStateCehckCount = 0;
	TailwindCount = 0;
	TailwindTimeOut = 0;
	AlignmentCount = 0;
	BreakCount = 0;
	BreakPwmDuty = 0;
	CloseLoopFlag = 0;
	BumplessCount = 0;
	
	
}
/*---------------------------------------------------------------------------*/
/* Name        : void MotorCloseLoop_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void MotorCloseLoop_Fun (void)
{
	if(CloseLoopFlag == 1)
	{
		PLL_KP_MACRO(Pll_Kp_Final);
		PLL_KI_MACRO(Pll_Ki);
		IQ_KP_MACRO(Iq_Kp);
		IQ_KI_MACRO(Iq_Ki);
		ID_KP_MACRO(Id_Kp);
		ID_KI_MACRO(Id_Ki);
		#if (CONTROL_MODE == Current_Control)
			MOTOR_CONT1 |= FOCANGSEL;// Use SMO_Ang
			IQ_CMD_MACRO(IQ_FINAL_START_VALUE);
		#endif
		#if (CONTROL_MODE == Speed_Control) || (CONTROL_MODE == Power_Limit)
			SPEED_UI_MACRO(IQ_FINAL_START_VALUE);
			//SPEED_UI_MACRO(0);
			FOCCONT |= SPEEDEN;
			MOTOR_CONT1 |= FOCANGSEL;// Use SMO_Ang
			MOTOR_CONT1 |= IQINSEL;// Use SPEED_PI_OUT
			SPEED_KP_MACRO(Spd_Kp_Final);
			SPEED_KI_MACRO(Spd_Ki);
			SPEED_CMD_MACRO(SpeedCmdTemp);
		#endif
		#if (CONTROL_MODE == Power_Limit)
			Watt = 0;
			Watt_C = 0;
		#endif
		#if (CONTROL_MODE == Power_Control)
			Watt = 0;
			Watt_C = 0;
			PIParm_Watt.Sum = ((signed long)IQ_FINAL_START_VALUE << 15);
		#endif
		FOCCONT |= PLLEN;
		//ID_CMD_MACRO(ID_FINAL_START_VALUE);
		MOTOR_CONT2 |= MOTOR_CONT2_REGS;
		MotorState = M_RUN;
	}
}

#if (BEMF_ONE_TAILWIND_FUNCTION == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void TailWindDetect_OneBEMF_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
#define dGain (unsigned short)((float) 32767 * 120/POLE/BASE_RPM)
void TailWindDetect_OneBEMF_Fun (void) 
{
		//BmfU = Adc_Cannel(BEMF_CH);
		if((BmfU > 70) && (ZeroPointFlag == 0))
		{
			if(++ZeroPointDebounceCnt > 10)
			{
				ZeroPointDebounceCnt = 0;
				ZeroPointFlag = 1;
			}
		}
		else if((BmfU < 70) && (ZeroPointFlag == 1))
		{
			if(++ZeroPointDebounceCnt > 10)
			{
				ZeroPointDebounceCnt = 0;
				ZeroPointFlag = 0;
			}
		}
		
		//P2_7 = ZeroPointFlag;
		
		HallUEdgeCnt++;
		if(HallUEdgeCnt >= PWM_Frequency)// 1Hz = 20000 * 50us 
		{
			HallUEdgeCnt = PWM_Frequency;
			RecHallUEdgeCnt = HallUEdgeCnt;
			ZeroPointCnt = 0;
		}
		
		if(ZeroPointFlag != ZeroPointFlagOld)
		{
			TailwindTimeOut = 0;
			if(ZeroPointFlag == 1)
			{
				SFR_PAGE = 1; CAPCONT |= 0x10;
				SFR_PAGE = 1; CaptureTotal = (CAPT_H * 256) + CAPT_L;
				CaptureTotalTemp = CaptureTotal;
				ZeroPointCnt++;
				if(ZeroPointCnt > 4)
				{
					ZeroPointCnt = 0;
					MotorDir = 1;
				}
			}
			else
			{
				SFR_PAGE = 1; CAPCONT &= ~(0x10);
				RecHallUEdgeCnt = HallUEdgeCnt;
				HallUEdgeCnt = 1;
				if(MotorDir == 1)
				{
					LatestTheta = (short)BEMF_CH_LATEST_THETA<<6;// U_CH
					#if 1 // Disable ZeroPointCheck
					if((RecHallUEdgeCnt > BEMF_TAILWIND_SPEED_MAX) && (RecHallUEdgeCnt < BEMF_TAILWIND_SPEED_MIN))
					{
						#if (TAILWIND_BREAK == 1)
							MotorState = M_BMF_BREAK;
						#else
							#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
								PllOutTemp = (PWM_Frequency / RecHallUEdgeCnt) * dGain;
								ZeroPointCheck = 1;
							#endif
						#endif
					}
					#endif
				}
			}
		}
		ZeroPointFlagOld = ZeroPointFlag;
		
		if(ZeroPointCheck)
		{
			if(MotorDir == 1)
			{
				VQ_OFFSET_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
				PLL_KP_MACRO(Pll_Kp_TailWind);
				PLL_KI_MACRO(Pll_Ki);
				ID_KP_MACRO(Id_Kp);
				ID_KI_MACRO(Id_Ki);
			}
			else if(MotorDir == 2)
			{
				VQ_OFFSET_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
				PLL_KP_MACRO(Pll_Kp_HeadWind);
				PLL_KI_MACRO(Pll_Ki);
				ID_KP_MACRO(Id_Kp);
				ID_KI_MACRO(Id_Ki);
			}
			IQ_KP_MACRO(Iq_Kp);
			IQ_KI_MACRO(Iq_Ki);

			MOTOR_CONT1 |= (FOCANGSEL);// Use SMO_Ang

			IQ_CMD_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
			//SPEED_UI_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
			//SPEED_OUT_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
			//ID_CMD_MACRO(ID_FINAL_TAILWIND_START_VALUE);
			//IQ_UI_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
			//IQ_OUT_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
			//ID_UI_MACRO(0);
			//ID_OUT_MACRO(0);
			RampDelayCount = 0;
			//CloseLoopDelayCount = 0;
			FOCCONT |= PLLEN;
			MOTOR_CONT1 |= (MPWMSEL);// Enable PWM Smo Mode
			MOTOR_CONT2 |= MOTOR_CONT2_REGS;// Enable D_SVPWM
			PWM_SetActive();
			FOC_ANG_MACRO(LatestTheta);
			CPU_ANG_MACRO(LatestTheta>>6);
			//PLL_UI_MACRO(PllOutTemp);
			//PLL_OUT_MACRO(PllOutTemp);
			MotorState = M_START;
		}
}
#endif

#if (BEMF_TWO_TAILWIND_FUNCTION_RES == 1) || (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void TailWindDetect_TwoBEMF_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
#define dGain (unsigned short)((float) 32767 * 120/POLE/BASE_RPM)
void TailWindDetect_TwoBEMF_Fun (void)
{
		//BmfV = Adc_Cannel(BEMF_V_CH);
		//BmfW = Adc_Cannel(BEMF_W_CH);
		BmfVW_Sub = BmfV - BmfW;
		//==============================
		// TailWind Slow Speed 
		//==============================
		#if (SLOW_SPEED == 1)
			if(abs(BmfVW_Sub) > 4) SlowSpeedFlag = 1;
			else                   SlowSpeedFlag = 0;
		#endif
		//==============================
		if((BmfVW_Sub > -20) && (ZeroPointFlag == 0))
		{
			if(++ZeroPointDebounceCnt > 10)
			{
				ZeroPointDebounceCnt = 0;
				ZeroPointFlag = 1;
			}
		}
		else if((BmfVW_Sub < -20) && (ZeroPointFlag == 1))
		{
			if(++ZeroPointDebounceCnt > 10)
			{
				ZeroPointDebounceCnt = 0;
				ZeroPointFlag = 0;
			}
		}
		
		//P2_7 = ZeroPointFlag;
		
		HallUEdgeCnt++;
		if(HallUEdgeCnt >= PWM_Frequency)// 1Hz = 20000 * 50us 
		{
			HallUEdgeCnt = PWM_Frequency;
			RecHallUEdgeCnt = HallUEdgeCnt;
			ZeroPointCnt = 0;
		}
		
		if(ZeroPointFlag != ZeroPointFlagOld)
		{
			TailwindTimeOut = 0;
			if(ZeroPointFlag == 1)
			{
				//SFR_PAGE = 1; CAPCONT |= 0x10;
				//SFR_PAGE = 1; CaptureTotal = (CAPT_H * 256) + CAPT_L;
				CaptureTotalTemp = CaptureTotal;
				BmfVW_Sum_Pos_ZeroPoint = BmfV + BmfW;
				if(BmfVW_Sum_Pos_ZeroPoint > BmfVW_Sum_Neg_ZeroPoint)
				{
					ZeroPointCnt++;
					if(ZeroPointCnt > 4)
					{
						ZeroPointCnt = 0;
						#if (BEMF_TWO_TAILWIND_FUNCTION_RES == 1)
								if(CCWFlag == 1)
									MotorDir = 1;
								else
									MotorDir = 2;
						#endif
						#if (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 1)
								if(CCWFlag == 1)
									MotorDir = 1;
								else
									MotorDir = 2;
						#endif
						TailwindTimeOut = 0;
					}
				}
				else if(BmfVW_Sum_Pos_ZeroPoint < BmfVW_Sum_Neg_ZeroPoint)
				{
					ZeroPointCnt--;
					if(ZeroPointCnt < -4)
					{
						ZeroPointCnt = 0;
						#if (BEMF_TWO_TAILWIND_FUNCTION_RES == 1)
								if(CCWFlag == 1)
									MotorDir = 2;
								else
									MotorDir = 1;
						#endif
						#if (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 1)
								if(CCWFlag == 1)
									MotorDir = 2;
								else
									MotorDir = 1;
						#endif
						TailwindTimeOut = 0;
					}
				}
			}
			else
			{
				//SFR_PAGE = 1; CAPCONT &= ~(0x10);
				RecHallUEdgeCnt = HallUEdgeCnt;
				HallUEdgeCnt = 1;
				BmfVW_Sum_Neg_ZeroPoint = BmfV + BmfW;
				if(MotorDir == 1)
				{
					#if (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 1)
							if(CCWFlag == 1)
								LatestTheta = (short)96<<6;
							else
								LatestTheta = (short)128<<6;// 64 96 128
					#endif
					#if (BEMF_TWO_TAILWIND_FUNCTION_RES == 1)
							if(CCWFlag == 1)
								LatestTheta = (short)0<<6;
							else
								LatestTheta = (short)64<<6;// 64 96 128
					#endif
					//LatestTheta = (short)64<<6;
					//LatestTheta = (short)128<<6;
					//LatestTheta = (short)192<<6;
					//LatestTheta = (short)256<<6;
					//LatestTheta = (short)320<<6;
					//LatestTheta = (short)382<<6;
					#if 1 // Disable ZeroPointCheck CW
						if((RecHallUEdgeCnt < BEMF_HEADWIND_SPEED_MAX)){
							// Wait 
						}
						else if((RecHallUEdgeCnt > BEMF_TAILWIND_SPEED_MAX) && (RecHallUEdgeCnt < BEMF_TAILWIND_SPEED_MIN))
						{
							#if (TAILWIND_BREAK == 1)
								MotorState = M_BMF_BREAK;
							#else
								#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
									PllOutTemp = (PWM_Frequency / RecHallUEdgeCnt) * dGain; // (PWM_Frequency / RecHallUEdgeCnt) * ((120/pole)/BaseRpm) * 32767
									ZeroPointCheck = 1;
								#endif
							#endif
						}
//						else if((RecHallUEdgeCnt < BEMF_TAILWIND_SPEED_MAX))
						else if((RecHallUEdgeCnt > BEMF_TAILWIND_SPEED_MIN))
						{
								MotorState = M_BMF_BREAK;
						}
					#endif
				}
				else if(MotorDir == 2)
				{
					#if (BEMF_TWO_TAILWIND_FUNCTION_DIODE == 1)
							if(CCWFlag == 1)
								LatestTheta = (short)320<<6;
							else
								LatestTheta = (short)288<<6;
					#endif
					#if (BEMF_TWO_TAILWIND_FUNCTION_RES == 1)
							if(CCWFlag == 1)
								LatestTheta = (short)288<<6;
							else
								LatestTheta = (short)192<<6;
					#endif
					#if 1 // Disable ZeroPointCheck CCW
						if((RecHallUEdgeCnt < BEMF_HEADWIND_SPEED_MAX)){
								// Wait
						}
						else if((RecHallUEdgeCnt > BEMF_HEADWIND_SPEED_MAX) && (RecHallUEdgeCnt < BEMF_HEADWIND_SPEED_MIN))
						{
							#if (HEADWIND_BREAK == 1)
								MotorState = M_BMF_BREAK;
							#else
								#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
									if(CCWFlag == 1) MOTOR_CONT2 &= ~(0x08); // Count clockwise direction FLAG  
									else             MOTOR_CONT2 |= (0x08);
									//PllOutTemp = (PWM_Frequency / RecHallUEdgeCnt); // for MDRFxx
									PllOutTemp = (PWM_Frequency / RecHallUEdgeCnt) * dGain;
									PllOutTemp = PllOutTemp;
									ZeroPointCheck = 1;
								#endif
							#endif
						}
						//else if((RecHallUEdgeCnt < BEMF_HEADWIND_SPEED_MAX))
						else if((RecHallUEdgeCnt > BEMF_TAILWIND_SPEED_MIN))
						{
							//#if (HEADWIND_BREAK == 1)
								MotorState = M_BMF_BREAK;
							//#endif
						}
					#endif
				}
			}
		}
		ZeroPointFlagOld = ZeroPointFlag;
		
		if(ZeroPointCheck)
		{
			if(MotorDir == 1)
			{
				VQ_OFFSET_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
				PLL_KP_MACRO(Pll_Kp_TailWind);
				PLL_KI_MACRO(Pll_Ki);
				ID_KP_MACRO(Id_Kp);
				ID_KI_MACRO(Id_Ki);
			}
			else if(MotorDir == 2)
			{
				VQ_OFFSET_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
				PLL_KP_MACRO(Pll_Kp_HeadWind);
				PLL_KI_MACRO(Pll_Ki);
				ID_KP_MACRO(Id_Kp);
				ID_KI_MACRO(Id_Ki);
			}
			IQ_KP_MACRO(Iq_Kp);
			IQ_KI_MACRO(Iq_Ki);

			MOTOR_CONT1 |= (FOCANGSEL);// Use SMO_Ang

			IQ_CMD_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
			//SPEED_UI_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
			//SPEED_OUT_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
			//ID_CMD_MACRO(ID_FINAL_TAILWIND_START_VALUE);
			//IQ_UI_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
			//IQ_OUT_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
			//ID_UI_MACRO(0);
			//ID_OUT_MACRO(0);
			RampDelayCount = 0;
			//CloseLoopDelayCount = 0;
			FOCCONT |= PLLEN;
			MOTOR_CONT1 |= (MPWMSEL);// Enable PWM Smo Mode
			MOTOR_CONT2 |= MOTOR_CONT2_REGS;
			PWM_SetActive();
			FOC_ANG_MACRO(LatestTheta);
			CPU_ANG_MACRO(LatestTheta>>6);
			//PLL_UI_MACRO(PllOutTemp);
			//PLL_OUT_MACRO(PllOutTemp);
			MotorState = M_START;
		}
}
#endif
/*---------------------------------------------------------------------------*/
/* Name        : void TailWindStart_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
short ZeroVector;
void TailWindStart_Fun (void)
{
	short Estimated_Spd;
	GET_SPEED_MACRO(Estimated_Spd);
	if(MotorDir == 1)
	{
		#if 1
		RampDelayCount++;
		if(RampDelayCount >= SMO_RAMP)
		{
			RampDelayCount = 0;
			if(PllOutTemp < FREQ_FINAL_START_VALUE)
			{
				PllOutTemp = Ramp_Fun(FREQ_FINAL_START_VALUE,PllOutTemp,32767,-32767,1);
			}
		}
		#endif
		
		#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
		PLL_UI_MACRO(PllOutTemp);
		//PLL_OUT_MACRO(PllOutTemp);
	
		#if (SOP_LEVEL == 2)
			//CloseLoopDelayCount++;
			//if(CloseLoopDelayCount > SMO_DELAY_DURATION)
			//{
				//CloseLoopDelayCount = 0;
				CloseLoopFlag = 1;
			//}
		#endif
		#endif
	}
	else if(MotorDir == 2)
	{
	#if (BEMF_ONE_TAILWIND_FUNCTION == 0)
		#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
		// =============================================
		// speed down for headwind procedure for MDRFxx
		RampDelayCount++;
		if(RampDelayCount >= 1){
			RampDelayCount = 0;
			PllOutTemp = Ramp_Fun(0,PllOutTemp,32767,-32767,2);
		}
		
		if(PllOutTemp <= 1)
		{
			if(ZeroVector == 1)
			{
				ZeroVector = 0;
				SFR_PAGE = 6; LatestTheta = SMO_D1;
				FOC_ANG_MACRO(LatestTheta);
				CPU_ANG_MACRO(LatestTheta>>6);
				// ===== IPDStart_Fun ======
				MotorDir = 0;
				MotorState = M_START;
				VQ_OFFSET_MACRO(0); //  Intial value,  IQ ID in PID controller can be filled in MDRFxx
				//VD_OFFSET_MACRO(0);
				//ResetMOC(); // reset IQ ID PID
				// ==============================
			}
			else
			{
				PWM_SetAllOff();
				if(CCWFlag == 1) MOTOR_CONT2 |= (0x08);
				else             MOTOR_CONT2 &= ~(0x08);
				ZeroVector = 1;
			}
		}
		PLL_UI_MACRO(PllOutTemp);
		//PLL_OUT_MACRO(PllOutTemp);
		// =============================================
		
		#endif
	#endif
	}
}
/*---------------------------------------------------------------------------*/
/* Name        : void Ramp_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
short Ramp_Fun (short TargetValue,SetpointValue,HighLimit,LowLimit,SetpValue)
{
	if(TargetValue != SetpointValue)
	{
		if(TargetValue >= SetpointValue)
		{
			SetpointValue += SetpValue;
			if(SetpointValue >= TargetValue)
				SetpointValue = TargetValue;
		}
		else
		{
			SetpointValue -= SetpValue;
			if(SetpointValue <= TargetValue)
				SetpointValue = TargetValue;
		}
	}
	
	if(SetpointValue > HighLimit)
		SetpointValue = HighLimit;
	else if(SetpointValue < LowLimit)
		SetpointValue = LowLimit;

	return SetpointValue;
}

#if (IPD_FUNCTION == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void IPDDetect_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
xdata unsigned char IPD_Cnt = 0;
xdata unsigned char IPD_Detect_State = 0;
void IPDDetect_Fun (void)
{
	#define IPDAdvanceAng 32
	switch (IPD_Detect_State)
	{
		case 0:
			AOCPCONT = IPD_LEVEL;
			Break_Fun();
			IPD_Cnt++;
			if(IPD_Cnt > 30)
			{
				IPD_Detect_State = 1;
				IPD_Cnt = 0;
			}
			break;
		case 1:
			//WatchDog_Disable(); // (note:IPD will trigger WatchDog Reset , Disable WatchDog)
			IPD_Init();
			//WatchDog_Init();
			IPD_Detect_State = 2;
			break;
		case 2:
			if(IPDPattern == 4) LatestTheta = (64+IPDAdvanceAng)<<6;
			else if(IPDPattern == 5) LatestTheta = (128+IPDAdvanceAng)<<6;
			else if(IPDPattern == 2) LatestTheta = (192+IPDAdvanceAng)<<6;
			else if(IPDPattern == 3) LatestTheta = (256+IPDAdvanceAng)<<6;
			else if(IPDPattern == 6) LatestTheta = (320+IPDAdvanceAng)<<6;
			else if(IPDPattern == 1) LatestTheta = (0+IPDAdvanceAng)<<6;
			else LatestTheta = (0+IPDAdvanceAng)<<6;
			MotorErrorState &= ~(AOCP);
			MotorState = M_START;
			IPD_Detect_State = 0;
			break;
		default:
			break;
	}
}
#endif
/*---------------------------------------------------------------------------*/
/* Name        : void IPDStart_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void IPDStart_Fun (void)
{
	static xdata float f_Temp;
	//short Estimated_Spd;
	//float f_PllTemp;
	//GET_SPEED_MACRO(Estimated_Spd);
	if(AlignmentCount < 1)
	{
		AlignmentCount++;
		//MOTOR_CONT2 |= (0x10);// Enable D_SVPWM
		MOTOR_CONT2 &= ~(0x10);// Disable D_SVPWM
		ResetMOC(); // Init PI 
		PllOutTemp = 0;
		//PLL_KP_MACRO(Pll_Kp_Start);
		//PLL_KI_MACRO(Pll_Ki);
		PLL_KP_MACRO(0);
		PLL_KI_MACRO(0);
		SPEED_KP_MACRO(0);
		SPEED_KI_MACRO(0);
		IQ_KP_MACRO(Iq_Kp);
		IQ_KI_MACRO(Iq_Ki);
		ID_KP_MACRO(0);
		ID_KI_MACRO(0);
		//ID_KP_MACRO(Id_Kp);
		//ID_KI_MACRO(Id_Ki);

		MOTOR_CONT1 |= FOCANGSEL;// Use SMO_Ang

		IqCmdTemp = IQ_INIT_START_VALUE;
		//IdCmdTemp = 0;
		IQ_CMD_MACRO(IqCmdTemp);
		//ID_CMD_MACRO(IdCmdTemp);
		RampDelayCount = 0;
//		CloseLoopDelayCount = 0;
		FOCCONT |= PLLEN;
		#if (IPD_FUNCTION == 1)
		FOC_ANG_MACRO(LatestTheta);
		CPU_ANG_MACRO(LatestTheta>>6);
		#else
		FOC_ANG_MACRO(LatestTheta);
		CPU_ANG_MACRO(LatestTheta>>6);
		#endif
		MOTOR_CONT1 |= (MPWMSEL);// Enable PWM Smo Mode
		PWM_SetActive();
		
		f_Temp = IQ_INIT_START_VALUE;
		//f_PllTemp = 0;
	}
	else if(AlignmentCount < I_ALIG_DURATION)
	{
		AlignmentCount++;
		f_Temp += I_ALIG_STEP_VALUE;
		//if(f_Temp >= IQ_FIRST_START_VALUE) f_Temp = IQ_FIRST_START_VALUE;
		IqCmdTemp = f_Temp;
		IQ_CMD_MACRO(IqCmdTemp);
		PllOutTemp = FREQ_FIRST_START_VALUE;
		PLL_UI_MACRO(PllOutTemp);
		//PLL_OUT_MACRO(PllOutTemp);
	}
	else if(AlignmentCount >= I_ALIG_DURATION)
	{
		//AlignmentCount = I_ALIG_DURATION;
		#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
		RampDelayCount++;
		if(RampDelayCount >= SMO_RAMP)
		{
			RampDelayCount = 0;
			PllOutTemp = Ramp_Fun(FREQ_FINAL_START_VALUE,PllOutTemp,FREQ_FINAL_START_VALUE,FREQ_FIRST_START_VALUE,PLL_ACC);

			IqCmdTemp = PllOutTemp * 14;
			if(IqCmdTemp > IQ_FINAL_START_VALUE) IqCmdTemp = IQ_FINAL_START_VALUE;
			//IqCmdTemp = Ramp_Fun(IQ_FINAL_START_VALUE,IqCmdTemp,32767,-32767,16);
			//IdCmdTemp = Ramp_Fun(ID_FINAL_START_VALUE,IdCmdTemp,32767,-32767,8);
			IQ_CMD_MACRO(IqCmdTemp);
			//ID_CMD_MACRO(IdCmdTemp);
		}
		
		
		PLL_UI_MACRO(PllOutTemp);
		//PLL_OUT_MACRO(PllOutTemp);

		
		#if (SOP_LEVEL == 2)
			if(
				(PllOutTemp >= FREQ_FINAL_START_VALUE)
				//(Estimated_Spd >= SPEED_OPEN_TO_COLSE_VALUE)
				//&& (IqCmdTemp >= IQ_FINAL_START_VALUE)
				&& (IqCmdTemp == IQ_FINAL_START_VALUE)
				)
			{
				//CloseLoopDelayCount++;
				//if(CloseLoopDelayCount > SMO_DELAY_DURATION)
				//{
					//CloseLoopDelayCount = 0;
					CloseLoopFlag = 1;
				//}
			}
		#endif
		#endif
	}
	else
	{
		AlignmentCount++;
	}
}
/*---------------------------------------------------------------------------*/
/* Name        : void Break_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
//void Break_Fun (void)
//{
//	#if (BREAK_FUNCTION == 1)
//		MOTOR_CONT1 &= ~(MPWMSEL);// Enable PWM User Mode
//		PWM_SetBreak();
//		if(BreakPwmDuty < BREAK_DUTY_VALUE) BreakPwmDuty++;
//		PWM_Duty(BreakPwmDuty);
//	#else
//		MOTOR_CONT1 &= ~(MPWMSEL);// Enable PWM User Mode
//		PWM_SetBreakForce();
//		if(BreakPwmDuty < BREAK_DUTY_VALUE) BreakPwmDuty++;
//		PWM_Duty(BreakPwmDuty);
//	#endif
//}

#if (Vbus_Protect == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void Vbus_Protect_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void Vbus_Protect_Fun (unsigned short Adc_Vbus)
{
	if(Adc_Vbus > OVER_BUS_VOLT_VALUE)
	{
		if(OverVbusProtectCount >= BUS_VOLT_DURATION)
		{
			OverVbusProtectCount = BUS_VOLT_DURATION; 
			MotorErrorState |= OverVbus;
		}
		else
			OverVbusProtectCount += Timer1_10ms;
	}
	else
	{
		if(Adc_Vbus < CLEAR_OVER_BUS_VOLT_VALUE)
		{
			if(OverVbusProtectCount < 1)
			{
				OverVbusProtectCount = 0;
				MotorErrorState &= ~(OverVbus);
			}
			else
				OverVbusProtectCount -= Timer1_10ms;
		}
	}

	if(Adc_Vbus < UNDER_BUS_VOLT_VALUE)
	{
		if(UnderVbusProtectCount >= BUS_VOLT_DURATION)
		{
			UnderVbusProtectCount = BUS_VOLT_DURATION; 
			MotorErrorState |= UnderVbus;
		}
		else
			UnderVbusProtectCount += Timer1_10ms;
	}
	else
	{
		if(Adc_Vbus > CLEAR_UNDER_BUS_VOLT_VALUE)
		{
			if(UnderVbusProtectCount < 1)
			{
				UnderVbusProtectCount = 0;
				MotorErrorState &= ~(UnderVbus);
			}
			else
				UnderVbusProtectCount -= Timer1_10ms;
		}
	}
}
#endif

#if (Temperture_Protect == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void Temperture_Protect_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
xdata int Temperture_I = 0;
void Temperture_Protect_Fun (unsigned short Adc_Temperture){
	int Err;
	// XNS20S73E6
	// 100 deg = 529, 95deg = 512 ,90deg = 429
	if(Adc_Temperture > OVER_TEMPERTURE_VALUE){
		OverTempertureProtectCount += Timer1_10ms;
		if(OverTempertureProtectCount >= TEMPERTURE_DURATION){ // 500ms
			OverTempertureProtectCount = 0; 
			MotorErrorState |= OverTemperture;
		}
	}else if(Adc_Temperture > OVER_TEMPERTURE_LOAD_REDUCE_VALUE){
		OverTempertureProtectCount += Timer1_10ms;
		if(OverTempertureProtectCount >= 20){  // 20ms
			OverTempertureProtectCount = 0;
				Err = (int)(Adc_Temperture - OVER_TEMPERTURE_LOAD_REDUCE_VALUE)>>3;
				Temperture_I = Temperture_I + Err;
			#if (CONTROL_MODE == 0)
				// Current Control
				if(Temperture_I > (IQ_MAX_LIMIT_VALUE>>1)) Temperture_I = IQ_MAX_LIMIT_VALUE>>1;
				else if(Temperture_I < 0)                  Temperture_I = 0;
			#endif
			#if (CONTROL_MODE == 1)
				// Speed Control
				if(Temperture_I > (Spd_MaxLimit>>1)) Temperture_I = Spd_MaxLimit>>1;
				else if(Temperture_I < 0)            Temperture_I = 0;
			#endif
		}
	}else{
		OverTempertureProtectCount = 0;
		if(Adc_Temperture < CLEAR_OVER_TEMPERTURE_VALUE){
			MotorErrorState &= ~(OverTemperture);
			if(Temperture_I > 1) Temperture_I--;
		}
	}
}
#endif
/*---------------------------------------------------------------------------*/
/* Name        : void Heat_Temperture_Protect_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
#if (Heat_Temperture_Protect == 1)
void Heat_Temperture_Protect_Fun (unsigned short Adc_Temperture){
	if(Adc_Temperture >= HEAT_TEMPERTURE_OPEN_CIRCUIT_VALUE){
		OverHeatTempertureProtectCount += Timer1_10ms;
		if(OverHeatTempertureProtectCount >= HEAT_TEMPERTURE_DURATION){
			OverHeatTempertureProtectCount = 0;
			HeatErrorState |= HeatOpenCircuit;
		}
	}else if(Adc_Temperture < OVER_HEAT_TEMPERTURE_VALUE){
		OverHeatTempertureProtectCount += Timer1_10ms;
		if(OverHeatTempertureProtectCount >= HEAT_TEMPERTURE_DURATION){
			OverHeatTempertureProtectCount = 0;
			HeatErrorState |= HeatOverTemperture;
		}
	}else{
		OverHeatTempertureProtectCount = 0;
		HeatErrorState = 0;
	}
}
#endif
/*---------------------------------------------------------------------------*/
/* Name        : void AOCP_Protect_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void AOCP_Protect_Fun (void)
{
	if(MotorState == M_IPD)
	{
		MotorErrorState &= ~(AOCP);
	}
	else
		MotorErrorState |= AOCP;
}

#if (POCP_Protect == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void Phase_OCP_Protect_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void Phase_OCP_Protect_Fun (void)
{
	SFR_PAGE = 2; IaFb = FOC_D1;
	SFR_PAGE = 2; IbFb = FOC_D2;
	IcFb = -IaFb - IbFb;
	if((abs(IaFb) > PHASE_OCP_VALUE) || (abs(IbFb) > PHASE_OCP_VALUE) || (abs(IcFb) > PHASE_OCP_VALUE))
	{
		if(POCP_ProtectCount >= PHASE_OCP_DURATION)
		{
			POCP_ProtectCount = PHASE_OCP_DURATION; 
			MotorErrorState |= POCP;
		}
		else
			POCP_ProtectCount += Timer1_10ms;
	}
	else
	{
		if(POCP_ProtectCount < 1)
		{
			POCP_ProtectCount = 0;
			MotorErrorState &= ~(POCP);
		}
		else
			POCP_ProtectCount -= Timer1_10ms;
	}
}
#endif

#if (FaultLock_Protect == 1)
/*---------------------------------------------------------------------------*/
/* Name        : void FaultLock_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void FaultLock_Fun (void)
{
	if(MotorState == M_RUN)
	{
		if(MotorFaultDelayCount > FAULTLOCK_DELAY_DURATION)
		{
			if(EstimatedSpeed > OverSpeed)
			{
				MotorFaultState = 1;
				MotorErrorState |= FaultLock;
			}
			
			if(EstimatedSpeed < UnderSpeed)
			{
				MotorFaultState = 2;
				MotorErrorState |= FaultLock;
			}
		}
		else
			MotorFaultDelayCount += Timer1_10ms;
	}
	else
	{
		FaultCount = 0;
		MotorFaultDelayCount = 0;
		if(MotorState != M_ERROR)
			MotorFaultState = 0;
	}
}
#endif

#if (CONTROL_MODE == Power_Limit)
/*---------------------------------------------------------------------------*/
/* Name        : void PowerLimit_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
// Mode_0
#define DEC_IAMP (0.004 * I_AMPLIFIER)
// Mode_1
#define DEC_SAMP 1 //(short) ((float) 1 * 32767 / BASE_RPM) 
int PowerLimit = Spd_MaxLimit;
int Temp_I;
unsigned char CheckCount;
bit VarietyFlag;

void PowerCla_Fun (void)
{
	unsigned short Tmp;
	Tmp = (unsigned short)((float) Vbus_avg * Ibus_avg * dPOWER_GAIN);
	Watt = DATA_FILTER(Tmp, Watt_C, Watt, unsigned short, 4);
}

void PowerLimit_Fun (void)
{
	//int Err;
	//unsigned short Tmp;
	SFR_PAGE = 0; Ibus_avg = abs(FOC_D1);
	//Tmp = (unsigned short)((float) Vbus_avg * Ibus_avg * dPOWER_GAIN);
	//Watt = DATA_FILTER(Tmp, Watt_C, Watt, unsigned short, 4);
	#if (POWER_SOP == 2)
		// Check Power Variety
		#if 1
		if(Watt > WATT_LIMIT_VALUE && Ibus_avg > IQ_MAX_LIMIT_VALUE){
				IQ_MAX_MACRO(16000);
				VarietyFlag = 1;
		}
		
		if(VarietyFlag == 1){
			if(++CheckCount >= 255){
				CheckCount = 0;
				IQ_MAX_MACRO(32767);
				VarietyFlag = 0;
			}
		}
		#endif
		#if 1
			// Mode_1
			SpeedCmdTemp = Ramp_Fun(SpeedCmd,SpeedCmdTemp,SPEED_MAX_LIMIT_VALUE,0,1);
			//SpeedCmdTemp = SpeedCmd;
			#if 1
				Err = (int)(Watt - WATT_LIMIT_VALUE)>>1;
				Temp_I = Temp_I + Err;
				if(Temp_I > SPEED_MAX_LIMIT_VALUE) Temp_I = SPEED_MAX_LIMIT_VALUE;
				else if(Temp_I < 0)                Temp_I = 0;
			#else
				if(Watt > WATT_LIMIT_VALUE)
				{
					if(Temp_I < SPEED_MAX_LIMIT_VALUE)
						Temp_I += DEC_SAMP;
				}
				else
				{
					if(Temp_I > DEC_SAMP)
						Temp_I -= DEC_SAMP;
				}
			#endif
				//SpeedCmdTemp = SpeedCmd - Temp_I;
				SPEED_CMD_MACRO(SpeedCmdTemp - Temp_I);
		#else
			// Mode_0
			//SpeedCmdTemp = Ramp_Fun(SpeedCmd,SpeedCmdTemp,SPEED_MAX_LIMIT_VALUE,0,1);
			SpeedCmdTemp = SpeedCmd;
			SPEED_CMD_MACRO(SpeedCmdTemp);
			if(Watt > WATT_LIMIT_VALUE)
			{
				if(PowerLimit > DEC_IAMP)
					PowerLimit -= DEC_IAMP;
			}
			else
			{
				if(PowerLimit < Spd_MaxLimit)
					PowerLimit += DEC_IAMP;
			}
			SPEED_MAX_MACRO(PowerLimit);
		#endif
	#else
		CurrentCmdTemp = CurrentCmd;
		IQ_CMD_MACRO(CurrentCmdTemp);
	#endif
}
#endif

#if (CONTROL_MODE == Power_Control)
/*---------------------------------------------------------------------------*/
/* Name        : void PowerControl_Fun (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
unsigned char VarietyCheckCount;
bit VarietyFlag;
void PowerCla_Fun (void)
{
	unsigned short Tmp;
	Tmp = (unsigned short)((float) Vbus_avg * Ibus_avg * dPOWER_GAIN);
	Watt = DATA_FILTER(Tmp, Watt_C, Watt, unsigned short, 3);
}

void PowerControl_Fun (void)
{
	//unsigned short Tmp;
	SFR_PAGE = 0; Ibus_avg = abs(FOC_D1);
	#if (POWER_SOP == 2)
		// Check Power Variety
		#if 0
		if(Watt > WATT_LIMIT_VALUE && Ibus_avg > IQ_MAX_LIMIT_VALUE){
				IQ_MAX_MACRO(16000);
				VarietyFlag = 1;
		}
		
		if(VarietyFlag == 1){
			if(++VarietyCheckCount >= 50){
				VarietyCheckCount = 0;
				IQ_MAX_MACRO(32767);
				VarietyFlag = 0;
			}
		}
		#endif
		
		if(++PIParm_Watt.Cnt >= WattFreSet) // WattFreSet
		{
			PIParm_Watt.Cnt = 0;
		#if (Power_LookUpTable == 1) // VBUS compensation  
			PIParm_Watt.InRef = look1_binlx(Vbus_avg, rtConstP_P.uDLookupTable_bp01Data, rtConstP_P.uDLookupTable_tableData, 2);
		#else
			PIParm_Watt.InRef = WATT_LIMIT_VALUE;
		#endif
			PIParm_Watt.InMeas = Watt;
			CalcPI(&PIParm_Watt);
			IQ_CMD_MACRO(PIParm_Watt.Out);
		}
	#else
		CurrentCmdTemp = CurrentCmd;
		IQ_CMD_MACRO(CurrentCmdTemp);
	#endif
}
#endif
/*---------------------------------------------------------------------------*/
/* Name        : void ResetMOC (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void ResetMOC (void)
{
	FOCCONT |= 0x80;// Clear PI & SMO
	SFR_PAGE = 0;
	PI_KP = 0;
	PI_KI = 0;
	SFR_PAGE = 1;
	PI_KP = 0;
	PI_KI = 0;
	SFR_PAGE = 2;
	PI_KP = 0;
	PI_KI = 0;
	SFR_PAGE = 3;
	PI_KP = 0;
	PI_KI = 0;
}
/*---------------------------------------------------------------------------*/
/* Name        : void MOC_Init (void)
/* Input       : NO
/* Output      : NO
/* Description : 
/*---------------------------------------------------------------------------*/
void MOC_Init (void)
{
	MOTOR_CONT1 = MOTOR_CONT1_REGS;
	MOTOR_CONT2 &= ~(0x10);
	FOCCONT = FOCCONT_REGS | PICLEAR | INVADCD | ADCTRIG;
	PI_GAIN = PI_GAIN_REGS;
}

