File name
Commit message
Commit date
File name
Commit message
Commit date
File name
Commit message
Commit date
File name
Commit message
Commit date
//-------------------------------------------------------//
// Project Code : CodeLibrary
// File Name : SoapPll3ph.cpp
// Created on : 2019. 06. 07.
// Description :
// Author : KimJeongWoo
// Last modified Date :
//-------------------------------------------------------//
#include <PLL/SoapPll3ph.h>
#include "CodeLibMath.h"
//Synchronous Observer-Aided Preprocessing PLL
void sSoapPll::Init(float mTsamp, float mVsRatePeak, float mFreqRate)
{
Tprd = mTsamp;
Zeta = 1;
Wc = 2. * 5. * PI;
Kp = 2. * Zeta * Wc;
Ki = Wc * Wc;
k = 1.5;
rho = 1.;
c1 = 0.5 * (1. + rho) * k;
c2 = 0.5 * rho * k * k;
Reset(mVsRatePeak, mFreqRate);
}
void sSoapPll::Reset(float mVsRatePeak, float mFreqRate)
{
Integral = 0.;
Wehat = 2. * PI * mFreqRate;
Weff = 2. * PI * mFreqRate;
Thetahat = 0.;
Thetd = 0.;
VdePos = 0.;
VqePos = mVsRatePeak; //140.*INV_SQRT3*SQRT2;
Vdehat = 0.;
Vqehat = mVsRatePeak; //0.;
}
float sSoapPll::Run(float mVde, float mVqe, float mFreqRate, float mTsamp)
{
float coefficient = 0.;
float out = 0.;
coefficient = Wehat * mTsamp;
// Synchronous Observer-Aided Preprocessing (SOAP)
ErrVde = mVde - Vdehat;
VqePos -= c2 * coefficient * ErrVde;
Vdehat += 2. * coefficient * (c1 * ErrVde + mVqe - VqePos);
ErrVqe = mVqe - Vqehat;
VdePos += c2 * coefficient * ErrVqe;
Vqehat += 2. * coefficient * (c1 * ErrVqe - mVde + VdePos);
//PLL
Thetd = atan2(-VdePos, VqePos);
Integral += Ki * mTsamp * Thetd;
Wehat = Weff + Integral;
FreqOut = Wehat * 0.5 * INV_PI;
Thetahat += mTsamp * (Kp * Thetd + Wehat);
Thetahat = BOUND_PI(Thetahat);
out = Thetahat;
return out;
}
void sSoapPll::Preparing(float mVde, float mVqe, float mTheta, float mWeRate)
{
Wehat = mWeRate;
Thetahat = mTheta;
VqePos = Vqehat = mVqe;
VdePos = Vdehat = mVde;
Integral = 0;
}