dsp2812 pmsm foc之park clark 速度計算

//======================================================================================================
//計算繞組電流ia,ib,ic===>ialfa,ibeta===>id,iq，矢量變換
//======================================================================================================
		ialfa=ia;
		ibeta=_IQmpy(ia,_IQ(0.57735026918963))+_IQmpy(ib,_IQ(1.15470053837926));    3/2變換矩陣公式

		id = -_IQmpy(ialfa,Cosine) -_IQmpy(ibeta,Sine);           // 旋轉變換矩陣公式這些都是標麼值在計算
		iq = -_IQmpy(ibeta,Cosine)+_IQmpy(ialfa,Sine) ; 
		ID=_IQtoF(_IQmpy(id,_IQ(E_Ding_DianLiu)));//標麼到實際    因爲是線性所以可以直接用
                IQ=_IQtoF(_IQmpy(iq,_IQ(E_Ding_DianLiu)));//標麼到實際
	    if (SpeedLoopCount>=SpeedLoopPrescaler)              //SpeedLoopPrescaler=10 也就是說10ms處理一次如下事件
	    {   

			DirectionQep = 0x4000&EvaRegs.GPTCONA.all; // 旋轉方向判定
			DirectionQep = DirectionQep>>14;

 			NewRawTheta =_IQ(EvaRegs.T2CNT);
// 計算機械角度
			if(DirectionQep ==1) //T2STAT=1,遞增計數；
			{
               
				RawThetaTmp =  OldRawTheta-NewRawTheta ; 
				if(RawThetaTmp > _IQ(0))
				{
				 RawThetaTmp = RawThetaTmp - TotalPulse;  
				}
			}
			else if(DirectionQep ==0) //T2STAT=0，遞減計數
			{
			 	RawThetaTmp =OldRawTheta- NewRawTheta; 
				if(RawThetaTmp < _IQ(0))
				{
				 RawThetaTmp = RawThetaTmp + TotalPulse;
				}    
			}
			Speed = _IQmpy(RawThetaTmp,SpeedScaler);    //速度計算,詳見函數分析小節
			SpeedRpm = _IQmpy(BaseRpm,Speed);   				
			OldRawTheta = NewRawTheta;
            if(Speed<0)
			{speed_dis=_IQtoF(_IQmpy(Speed, _IQ(-100)));} //乘以100只是爲了顯示，因爲speed是標麼值。
			else{
            speed_dis=_IQtoF(_IQmpy(Speed, _IQ(100)));}
		    SpeedLoopCount=1; 
			RawThetaTmp=0;