/*=====================================================================================
File name: PLLCALC.C (IQ version)
Originator: Digital Control Systems Group
Texas Instruments
Description: Voltage Phase Estimator of 3 Phase AC System
=====================================================================================
History:
-------------------------------------------------------------------------------------
09-14-2008 Version 1.00
-------------------------------------------------------------------------------------*/
#include "IQmathLib.h" // Include header for IQmath library
#include "dmctype.h"
#include "parameter.h"
#include "pll_calc.h"
#include "pid_reg3.h"
extern PIDREG3 pid1_uq;
void pll_calc(PLLCALC *v)
{
_iq Cosine,Sine,StepAngle,ThetaTemp;
_iq k=_IQ(0.0002273);
v->Ea=_IQmpy(_IQdiv((v->Ia-v->Ia1),v->T),k)+v->Va;
v->Eb=_IQmpy(_IQdiv((v->Ib-v->Ib1),v->T),k)+v->Vb;
v->Ia1=v->Ia;
v->Ib1=v->Ib;
v->Alpha = v->As;
v->Beta = _IQmpy((v->As + _IQmpy(_IQ(2),v->Bs)),_IQ(0.57735026918963));
v->MagSV = _IQmag(v->Alpha, v->Beta);
v->ThetaSV = _IQatan2PU(v->Beta,v->Alpha);
Sine = _IQsinPU(v->ThetaSVPLL);
Cosine = _IQcosPU(v->ThetaSVPLL);
v->Ds = _IQmpy(v->Alpha,Cosine) + _IQmpy(v->Beta,Sine);
v->Qs = _IQmpy(v->Beta,Cosine) - _IQmpy(v->Alpha,Sine);
pid1_uq.Ref = _IQ(0);
pid1_uq.Fdb = -v->Qs;
pid1_uq.calc(&pid1_uq);
v->Fcalc = pid1_uq.Out + v->Fb;
StepAngle = _IQmpy(v->Fcalc,v->StepAngleMax);
// Calculate the new angle alpha
v->ThetaSVPLL = v->ThetaSVPLL + StepAngle;
if (v->ThetaSVPLL >= _IQ(1.0))
v->ThetaSVPLL = v->ThetaSVPLL - _IQ(1.0);
//ThetaTemp = v->ThetaSVPLL-_IQ(0.08333333);
//if (ThetaTemp < _IQ(0))
// ThetaTemp = ThetaTemp + _IQ(1);
// v->ThetaSVPLL30Shift = ThetaTemp;
v->ThetaSVPLL30Shift =v->ThetaSVPLL-_IQ(0.08333333);
if (v->ThetaSVPLL30Shift<= _IQ(0))
v->ThetaSVPLL30Shift = v->ThetaSVPLL30Shift + _IQ(1);
v->MagSVPLL = _IQmag(v->Ds, v->Qs);
Cosine = _IQcosPU(v->ThetaSVPLL30Shift);
v->AsSV = _IQmpy(_IQmpy(v->MagSV,Cosine),_IQ(0.577350269));
ThetaTemp=v->ThetaSVPLL30Shift;
ThetaTemp = ThetaTemp-_IQ(0.33333333);
if (ThetaTemp < _IQ(0))
ThetaTemp = ThetaTemp + _IQ(1);
Cosine = _IQcosPU(ThetaTemp);
v->BsSV = _IQmpy(_IQmpy(v->MagSV,Cosine),_IQ(0.577350269));
v->AlfaSV = v->AsSV;
v->BetaSV = _IQmpy((v->AsSV + _IQmpy(_IQ(2),v->BsSV)),_IQ(0.577350269));
// Calculate Three_Phase Additional Singal //
//StepAngle = _IQmpy(_IQ(3.0),v->StepAngleMax);
StepAngle = _IQmpy(_IQmpy(_IQ(3.0),v->Fcalc),v->StepAngleMax);
//StepAngle = _IQmpy(_IQmpy(_IQ(5.0),v->Fcalc),v->StepAngleMax);
// Calculate new angle alpha
v->ThetaAdd = v->ThetaAdd + StepAngle;
if (v->ThetaAdd >= _IQ(1.0))
v->ThetaAdd = v->ThetaAdd - _IQ(1.0);
// Phase A Additional Singal
//Sine = _IQsinPU(v->ThetaAdd);
//v->AsAdd = _IQmpy(_IQ(0.1),Sine);
Cosine = _IQcosPU(v->ThetaAdd);
v->AsAdd = -_IQmpy(_IQ(0.2),Cosine);
// Phase B Additional Singal
ThetaTemp = v->ThetaAdd-_IQ(0.33333333);
if (ThetaTemp < _IQ(0))
ThetaTemp = ThetaTemp + _IQ(1);
//Sine = _IQsinPU(ThetaTemp);
//v->BsAdd = _IQmpy(_IQ(0.1),Sine);
Cosine = _IQcosPU(ThetaTemp);
v->BsAdd = -_IQmpy(_IQ(0.2),Cosine);
// Phase C Additional Singal
ThetaTemp = v->ThetaAdd+_IQ(0.33333333);
if (ThetaTemp > _IQ(1))
ThetaTemp = ThetaTemp - _IQ(1);
//Sine = _IQsinPU(ThetaTemp);
//v->CsAdd = _IQmpy(_IQ(0.1),Sine);
Cosine = _IQcosPU(ThetaTemp);
v->CsAdd = -_IQmpy(_IQ(0.2),Cosine);
}