//-------------------------------------------------------// // Project Code : CodeLibrary // File Name : SogiFll1ph.cpp // Created on : 2023. 7. 13. // Description : // Author : "Kim JeongWoo" // Last modified Date : //-------------------------------------------------------// #include <PLL/SogiFll1ph.h> #include "CodeLibMath.h" void sSogiFll1ph::Init(float mFreqRate, float mTsamp, float mVacPeakMax, float *Vac, float *Iac) { pVac = (float *) Vac; pIac = (float *) Iac; Vs_U[0] = 0.0; Vs_U[1] = 0.0; Vs_U[2] = 0.0; Is_U[0] = 0.0; Is_U[1] = 0.0; Is_U[2] = 0.0; Vs_Osg_U[0] = 0.0; Vs_Osg_U[1] = 0.0; Vs_Osg_U[2] = 0.0; Vs_Osg_Qu[0] = 0.0; Vs_Osg_Qu[1] = 0.0; Vs_Osg_Qu[2] = 0.0; Vs_u_Q[0] = 0.0; Vs_u_Q[1] = 0.0; Vs_u_D[0] = 0.0; Vs_u_D[1] = 0.0; Is_Osg_U[0] = 0.0; Is_Osg_U[1] = 0.0; Is_Osg_U[2] = 0.0; Is_Osg_Qu[0] = 0.0; Is_Osg_Qu[1] = 0.0; Is_Osg_Qu[2] = 0.0; Is_u_Q[0] = 0.0; Is_u_Q[1] = 0.0; Is_u_D[0] = 0.0; Is_u_D[1] = 0.0; Vs_ylf[0] = 0.0; Vs_ylf[1] = 0.0; Is_ylf[0] = 0.0; Is_ylf[1] = 0.0; FreqOut = 0.0; FreqRate = mFreqRate; Theta = 0.0; SinTheta = 0.0; CosTheta = 0.0; Tsamp = mTsamp; VacPeakMax = mVacPeakMax; x3[0] = 0; x3[1] = 0; ef2 = 0; WeRate = 2 * PI * FreqRate; WeHat = 2 * PI * FreqRate; SettlingTime = 0.03; //3ms DampingRatio = 0.7; ErrBand = 0.01; Wn = ((log((1 / sqrt(1 - DampingRatio * DampingRatio)) / ErrBand)) / SettlingTime) / DampingRatio; Ti = 2 * (DampingRatio / Wn); Kp = Wn * Wn * Ti; Ki = Kp / Ti; LpfCoefUpdate(); CoefUpdate(WeRate); } void sSogiFll1ph::LpfCoefUpdate() { // loop filter coefficients for 20kHz Lpf.b0 = ((2.0) * Kp + Ki * Tsamp) * 0.5 / VacPeakMax; Lpf.b1 = -((2.0) * Kp - Ki * Tsamp) * 0.5 / VacPeakMax; Lpf.a1 = 1.0; } void sSogiFll1ph::CoefUpdate(float mWe) { float temp; Osg.k = (float)(1.2); Osg.x = (float)(2.0 * Osg.k * mWe * Tsamp); Osg.y = (float)(mWe * Tsamp * mWe * Tsamp); temp = (float)1.0 / (Osg.x + Osg.y + 4.0); Osg.b0 = ((float)Osg.x * temp); Osg.b2 = ((float)(-1.0) * Osg.b0); Osg.a1 = ((float)(2.0 * (4.0 - Osg.y)) * temp); Osg.a2 = ((float)(Osg.x - Osg.y - 4) * temp); Osg.qb0 = ((float)(Osg.k * Osg.y) * temp); Osg.qb1 = (Osg.qb0 * (float)(2.0)); Osg.qb2 = Osg.qb0; } void sSogiFll1ph::Run(void) { Vs_U[0] = *pVac; Vs_Osg_U[0] = (Osg.b0 * (Vs_U[0] - Vs_U[2])) + (Osg.a1 * Vs_Osg_U[1]) + (Osg.a2 * Vs_Osg_U[2]); Vs_Osg_U[2] = Vs_Osg_U[1]; Vs_Osg_U[1] = Vs_Osg_U[0]; Vs_Osg_Qu[0] = ((Osg.qb0 * Vs_U[0]) + (Osg.qb1 * Vs_U[1]) + (Osg.qb2 * Vs_U[2]) + (Osg.a1 * Vs_Osg_Qu[1]) + (Osg.a2 * Vs_Osg_Qu[2])); Vs_Osg_Qu[2] = Vs_Osg_Qu[1]; Vs_Osg_Qu[1] = Vs_Osg_Qu[0]; Vs_U[2] = Vs_U[1]; Vs_U[1] = Vs_U[0]; Vs_u_D[0] = (CosTheta * Vs_Osg_U[0]) + (SinTheta * Vs_Osg_Qu[0]); Vs_u_Q[0] = (CosTheta * Vs_Osg_Qu[0]) - (SinTheta * Vs_Osg_U[0]); // Is_U[0] = *pIac; Is_Osg_U[0] = (Osg.b0 * (Is_U[0] - Is_U[2])) + (Osg.a1 * Is_Osg_U[1]) + (Osg.a2 * Is_Osg_U[2]); Is_Osg_U[2] = Is_Osg_U[1]; Is_Osg_U[1] = Is_Osg_U[0]; Is_Osg_Qu[0] = ((Osg.qb0 * Is_U[0]) + (Osg.qb1 * Is_U[1]) + (Osg.qb2 * Is_U[2]) + (Osg.a1 * Is_Osg_Qu[1]) + (Osg.a2 * Is_Osg_Qu[2])); Is_Osg_Qu[2] = Is_Osg_Qu[1]; Is_Osg_Qu[1] = Is_Osg_Qu[0]; Is_U[2] = Is_U[1]; Is_U[1] = Is_U[0]; Is_u_D[0] = (CosTheta * Is_Osg_U[0]) + (SinTheta * Is_Osg_Qu[0]); Is_u_Q[0] = (CosTheta * Is_Osg_Qu[0]) - (SinTheta * Is_Osg_U[0]); Vs_ylf[0] = Vs_ylf[1] + (Lpf.b0 * Vs_u_D[0]) + (Lpf.b1 * Vs_u_D[1]); Vs_ylf[1] = Vs_ylf[0]; Vs_u_D[1] = Vs_u_D[0]; FreqOut = (FreqRate - Vs_ylf[0]); WeHat = ((FreqOut) * (float(2.0 * PI))); Theta = Theta + WeHat * Tsamp; Theta = BOUND_PI(Theta); SinTheta = (float)sin(Theta); CosTheta = (float)cos(Theta); ef2 = ((Vs_U[0] - Vs_Osg_U[0]) * Vs_Osg_Qu[0]) * 0.7 * Tsamp * -1.0; x3[0] = x3[1] + ef2; x3[1] = x3[0]; WeHat = WeRate + x3[0]; FreqRate = WeHat / (2.0 * PI); CoefUpdate(WeHat); }