MC-State-Observer

MC_State_Observer/********************(C)COPYRIGHT2007STMicroelectronics*********************FileName:MC_State_Observer.c*Author:IMSSystemsLab*DateFirstIssued:21/11/07*Description:ThismoduleimplementstheStateObserverof*thePMSMB-EMF,thusidentifyingrotorspeedandposition**********************************************************************************History:*21/11/07v1.0*********************************************************************************THEPRESENTSOFTWAREWHICHISFORGUIDANCEONLYAIMSATPROVIDINGCUSTOMERS*WITHCODINGINFORMATIONREGARDINGTHEIRPRODUCTSINORDERFORTHEMTOSAVETIME.*ASARESULT,STMICROELECTRONICSSHALLNOTBEHELDLIABLEFORANYDIRECT,*INDIRECTORCONSEQUENTIALDAMAGESWITHRESPECTTOANYCLAIMSARISINGFROMTHE*CONTENTOFSUCHSOFTWAREAND/ORTHEUSEMADEBYCUSTOMERSOFTHECODING*INFORMATIONCONTAINEDHEREININCONNECTIONWITHTHEIRPRODUCTS.*******************************************************************************//*Includes------------------------------------------------------------------*/#includestm32f10x_type.h#includeMC_type.h#includeMC_State_Observer.h/*Privatedefine------------------------------------------------------------*/#defineBUFFER_SIZE(u8)64/*Privatevariables---------------------------------------------------------*/statics32wIalfa_est,wIbeta_est;statics32wBemf_alfa_est,wBemf_beta_est;statics32wSpeed_PI_integral_sum;statics16hBemf_alfa_est,hBemf_beta_est;statics16hSpeed_Buffer[BUFFER_SIZE];statics16hRotor_El_Angle;statics16hRotor_Speed_dpp;staticboolMax_Speed_Out,Min_Speed_Out,Is_Speed_Reliable;staticu8bSpeed_Buffer_Index;externconsts16hSin_Cos_Table[256];staticvolatiles16hC2,hC4;statics16hF1,hF2,hF3,hC1,hC3,hC5,hC6;staticvolatiles16hSpeed_P_Gain,hSpeed_I_Gain;statics32wMotorMaxSpeed_dpp;staticu16hPercentageFactor;/*Privatefunctionprototypes-----------------------------------------------*/s16Calc_Rotor_Speed(s16,s16);s16Speed_PI(s16,s16);voidStore_Rotor_Speed(s16);//functionofMC_Clarke_Park.cTrig_ComponentsTrig_Functions(s16);/********************************************************************************FunctionName:STO_Calc_Rotor_Angle角度*Description:ThisfunctionimplementstheStateObserverofa*PMSMback-emfs.Thus,motorspeedandrotoranglearecalculated*andstoredinthevariableshRotor_Speed,hRotor_El_Angle.*Input:StatorvoltagereferencevaluesValpha,Vbeta(asprovidedbyRevPark*throughvariableStat_Volt_alfa_beta),*StatorcurrentvaluesIalpha,Ibeta(asprovidedbyClarktrasformation*throughvariableStat_Curr_alfa_beta),DCbusvoltage.*Output:None*Return:None*******************************************************************************/voidSTO_Calc_Rotor_Angle(Volt_ComponentsStat_Volt_alfa_beta,Curr_ComponentsStat_Curr_alfa_beta,s16hBusVoltage){s32wIalfa_est_Next,wIbeta_est_Next;第1页MC_State_Observers32wBemf_alfa_est_Next,wBemf_beta_est_Next;s16hValfa,hVbeta;s16hIalfa_err,hIbeta_err;s16hRotor_Speed;s32bDirection;//////////////////限幅if(wBemf_alfa_est(s32)(S16_MAX*hF2)){wBemf_alfa_est=S16_MAX*hF2;}elseif(wBemf_alfa_est=(s32)(S16_MIN*hF2)){wBemf_alfa_est=-S16_MAX*hF2;}if(wBemf_beta_est(s32)(S16_MAX*hF2)){wBemf_beta_est=S16_MAX*hF2;}elseif(wBemf_beta_est=(s32)(S16_MIN*hF2)){wBemf_beta_est=-S16_MAX*hF2;}if(wIalfa_est(s32)(S16_MAX*hF1)){wIalfa_est=S16_MAX*hF1;}elseif(wIalfa_est=(s32)(S16_MIN*hF1)){wIalfa_est=-S16_MAX*hF1;}if(wIbeta_estS16_MAX*hF1){wIbeta_est=S16_MAX*hF1;}elseif(wIbeta_est=S16_MIN*hF1){wIbeta_est=-S16_MAX*hF1;}//////////////////限幅wdwd2008hIalfa_err=(s16)((wIalfa_est/hF1)-Stat_Curr_alfa_beta.qI_Component1);hIbeta_err=(s16)((wIbeta_est/hF1)-Stat_Curr_alfa_beta.qI_Component2);hValfa=(s16)((Stat_Volt_alfa_beta.qV_Component1*hBusVoltage)/32768);hVbeta=(s16)((Stat_Volt_alfa_beta.qV_Component2*hBusVoltage)/32768);/*alfaaxesobserver*/wIalfa_est_Next=(s32)(wIalfa_est-(s32)(hC1*(s16)(wIalfa_est/hF1))+(s32)(hC2*hIalfa_err)+(s32)(hC5*hValfa)-(s32)(hC3*(s16)(wBemf_alfa_est/hF2)));//I(n+1)=I(n)-rs*T/Ls*I(n)+K1*(I(n)-i(n))+T/Ls*V-T/Ls*emfwBemf_alfa_est_Next=(s32)(wBemf_alfa_est+(s32)(hC4*hIalfa_err)+(s32)(hC6*hRotor_Speed_dpp*(wBemf_beta_est/(hF2*hF3))));//emf(n+1)=emf(n)+K2*(I(n)-i(n))+p*w*emfb*T/*betaaxesobserver*/wIbeta_est_Next=(s32)(wIbeta_est-(s32)(hC1*(s16)(wIbeta_est/hF1))+(s32)(hC2*hIbeta_err)+(s32)(hC5*hVbeta)-(s32)(hC3*(s16)(wBemf_beta_est/hF2)));wBemf_beta_est_Next=(s32)(wBemf_beta_est+(s32)(hC4*hIbeta_err)-(s32)(hC6*hRotor_Speed_dpp*(wBemf_alfa_est/(hF2*hF3))));/*Extrapolationofpresentrotationdirection,necessaryforPLL*/if(hRotor_Speed_dpp=0){bDirection=1;}第2页MC_State_Observerelse{bDirection=-1;}/*CallsthePLLblockset*/hBemf_alfa_est=wBemf_alfa_est/hF2;hBemf_beta_est=wBemf_beta_est/hF2;hRotor_Speed=Calc_Rotor_Speed((s16)(hBemf_alfa_est*bDirection),(s16)(-hBemf_beta_est*bDirection));Store_Rotor_Speed(hRotor_Speed);hRotor_El_Angle=(s16)(hRotor_El_Angle+hRotor_Speed);/*storingpreviousvaluesofcurrentsandbemfs*/wIalfa_est=wIalfa_est_Next;wBemf_alfa_est=wBemf_alfa_est_Next;wIbeta_est=wIbeta_est_Next;wBemf_beta_est=wBemf_beta_est_Next;}/********************************************************************************Functio