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;