SMO-滑模观测器参考代码:
void FOC_SMO_Observer(struct_smo_est *pHandle)
{
//---------------------------------------------------------------------------------------------------/
/* Sliding mode current observer */
pHandle->Est_Ialpha = (((pHandle->Fs>>2) * (pHandle->Est_Ialpha)) + ((pHandle->Gs>>2) * (pHandle->Valpha - pHandle->Ealpha - pHandle->Zalpha)))>>13;
pHandle->Est_Ibeta = (((pHandle->Fs>>2) * (pHandle->Est_Ibeta)) + ((pHandle->Gs>>2) * (pHandle->Vbeta - pHandle->Ebeta - pHandle->Zbeta)))>>13;
//---------------------------------------------------------------------------------------------------/
/* Current errors */
pHandle->IalphaError = pHandle->Est_Ialpha - pHandle->Ialpha;
pHandle->IbetaError = pHandle->Est_Ibeta - pHandle->Ibeta;
//---------------------------------------------------------------------------------------------------/
/* Ialpha Sliding control calculator */
if (_IQabs(pHandle->IalphaError) < pHandle->E0)
pHandle->Zalpha = (pHandle->IalphaError*(pHandle->Kslide))/(pHandle->E0);
else if (pHandle->IalphaError > 0)
pHandle->Zalpha = pHandle->Kslide;
else
pHandle->Zalpha = -pHandle->Kslide;
//---------------------------------------------------------------------------------------------------/
/* Ibeta Sliding control calculator */
if (_IQabs(pHandle->IbetaError) < pHandle->E0)
pHandle->Zbeta = (pHandle->IbetaError*(pHandle->Kslide)/(pHandle->E0);
else if (pHandle->IbetaError > 0)
pHandle->Zbeta = pHandle->Kslide;
else
pHandle->Zbeta = -pHandle->Kslide;
//---------------------------------------------------------------------------------------------------/
/* Sliding control filter -> back EMF calculator */
pHandle->Ealpha = pHandle->Ealpha + _IQmpy(pHandle->Kslf,(pHandle->Zalpha-pHandle->Ealpha));
pHandle->Ebeta = pHandle->Ebeta + _IQmpy(pHandle->Kslf,(pHandle->Zbeta-pHandle->Ebeta));
//---------------------------------------------------------------------------------------------------/
/* QPLL得到估算转速 */
/* E(error) = -E(alfa)*Cosθ - E(beta)*Sinθ = K*▲θ */
stru_SinCos_PLL = FOC_SinCos_Calc(pHandle->Angle);
pHandle->PI_Error = ( ( ((-pHandle->Ealpha) * stru_SinCos_PLL.Cos) - (pHandle->Ebeta * stru_SinCos_PLL.Sin) )>>15);
pHandle->PI_Ui += (( pHandle->PI_Error * pHandle->PI_Ki ) >> 1 );
if ( pHandle->PI_Ui > (pHandle->PI_Upper << 14) ) pHandle->PI_Ui = pHandle->PI_Upper << 14;
else if ( pHandle->PI_Ui < (pHandle->PI_Lower << 14) ) pHandle->PI_Ui = pHandle->PI_Lower << 14;
pHandle->PI_Out = (( pHandle->PI_Error * pHandle->PI_Kp ) >> 15 ) + (pHandle->PI_Ui >> 14);
if ( pHandle->PI_Out > pHandle->PI_Upper ) pHandle->PI_Out = pHandle->PI_Upper;
else if ( pHandle->PI_Out < pHandle->PI_Lower ) pHandle->PI_Out = pHandle->PI_Lower;
//计算角度
pHandle->GWe = pHandle->PI_Out;
pHandle->Angle += pHandle->GWe;
/* 角度限制处理 */
if ( pHandle->Angle < 0 ) pHandle->Angle += 65535;
else if ( pHandle->Angle > 65535 ) pHandle->Angle -= 65535;
//---------------------------------------------------------------------------------------------------/
}