578 lines
12 KiB
C
578 lines
12 KiB
C
#include "IQmathLib.h"
|
|
#include "alg_pll.h"
|
|
#include "params_pll.h"
|
|
|
|
#include "params_norma.h"
|
|
|
|
//#define NORMA_ACP 3000
|
|
//#define FREQ_PWM_VIPR 1975
|
|
|
|
//#define SIZE_PLL_AVG 50
|
|
|
|
//_iq w_in_avg[SIZE_PLL_AVG];
|
|
//_iq w_out_avg[SIZE_PLL_AVG];
|
|
|
|
#define DETECT_PLL_D (2000.0/NORMA_ACP) // ampl
|
|
#define DETECT_PLL_Q (500.0/NORMA_ACP) // zero
|
|
|
|
_iq iqDetect_PLL_d=_IQ(DETECT_PLL_D);
|
|
_iq iqDetect_PLL_q=_IQ(DETECT_PLL_Q);
|
|
|
|
#define MAX_COUNT_ERR_PLL 5000 //20
|
|
|
|
#ifdef USE_SMOOTH_FOR_CALC_WC
|
|
SMOOTH mysmooth=SMOOTH_DEFAULTS;
|
|
#endif
|
|
|
|
|
|
//
|
|
///////////////////////////////////////////
|
|
//PLL_REC pll2 = PLL_REC_DEFAULT;
|
|
///////////////////////////////////////////
|
|
|
|
|
|
ABC_TO_ALPHABETA abc_to_alphabeta_u_input = ABC_TO_ALPHABETA_DEFAULTS;
|
|
ALPHABETA_TO_DQ alphabeta_to_dq_u_input = ALPHABETA_TO_DQ_DEFAULTS;
|
|
PIDREG3 pidUdq = PIDREG3_DEFAULTS;
|
|
|
|
//int count_wait_pll=0;
|
|
int count_err_pll = 0;
|
|
|
|
|
|
//int c_start=0;
|
|
//int c_stop=0;
|
|
|
|
#define MAX_TIME_WAIT_PLL 5000
|
|
|
|
|
|
//_iq iqUab=0, iqUbc=0, iqUca=0;
|
|
|
|
|
|
//_iq koef_Um_filter = _IQ(0.000125/0.09);
|
|
|
|
|
|
//_iq koef_AddIq_minus_1_filter = _IQ(0.00034/0.009);//9576244354248046875
|
|
|
|
#pragma CODE_SECTION(minus_plus_2_pi,".fast_run");
|
|
_iq minus_plus_2_pi(_iq a)
|
|
{
|
|
|
|
while (a>=CONST_IQ_2PI)
|
|
a -= CONST_IQ_2PI;
|
|
|
|
while (a<=-CONST_IQ_2PI)
|
|
a += CONST_IQ_2PI;
|
|
|
|
return a;
|
|
}
|
|
|
|
|
|
#pragma CODE_SECTION(minus_plus_2_pi_v2,".fast_run");
|
|
_iq minus_plus_2_pi_v2(_iq a)
|
|
{
|
|
|
|
while (a>=CONST_IQ_2PI)
|
|
a -= CONST_IQ_2PI;
|
|
|
|
while (a<0)
|
|
a += CONST_IQ_2PI;
|
|
|
|
return a;
|
|
}
|
|
|
|
|
|
|
|
#pragma CODE_SECTION(AB_BC_CA_To_ABC,".fast_run");
|
|
void AB_BC_CA_To_ABC(_iq U_AB, _iq U_BC, _iq U_CA, _iq *Ua, _iq *Ub, _iq *Uc)
|
|
{
|
|
// static _iq c2 = _IQ(2.0);
|
|
|
|
// static _iq c13_sqrt = _IQ(1.7320508075688772935274463415059 / 3.0);
|
|
|
|
*Ua = U_AB;
|
|
*Ub = U_BC;
|
|
*Uc = U_CA;
|
|
/*
|
|
|
|
|
|
*Ua = _IQmpy(c13_sqrt, (_IQmpy(c2,U_AB)+U_BC));// 2*U_AB/3+U_BC/3;
|
|
*Ub = _IQmpy(c13_sqrt, (_IQmpy(c2,U_BC)+U_CA));// 2*U_BC/3+U_CA/3;
|
|
*Uc = _IQmpy(c13_sqrt, (_IQmpy(c2,U_CA)+U_AB));// 2*U_CA/3+U_AB/3;
|
|
*/
|
|
}
|
|
|
|
|
|
|
|
|
|
/////////////////////////////////////////////////
|
|
#pragma CODE_SECTION(PLLController,".fast_run");
|
|
/////////////////////////////////////////////////
|
|
void PLLController(PLL_REC *v)
|
|
{
|
|
static unsigned int count_wait_pll=0;
|
|
static _iq prev_Tetta_z=0;
|
|
_iq Tetta_z_t=0;
|
|
static int prev_flag_find_pll = 0;
|
|
static int flag_reset_Tetta_p = 0;
|
|
// static _iq prev_Tetta_v2 = 0;
|
|
|
|
|
|
v->vars.Uab = v->input.Input_U_AB - v->vars.iqZeroUAB;
|
|
v->vars.Ubc = v->input.Input_U_BC - v->vars.iqZeroUBC;
|
|
v->vars.Uca = v->input.Input_U_CA - v->vars.iqZeroUCA;
|
|
|
|
v->vars.sum_zeroU_AB_BC_CA = v->vars.Uab + v->vars.Ubc + v->vars.Uca;
|
|
|
|
if (v->setup.rotation_u_cba)
|
|
{
|
|
AB_BC_CA_To_ABC(v->vars.Uab, v->vars.Uca, v->vars.Ubc, &v->vars.Ua, &v->vars.Ub, &v->vars.Uc);
|
|
}
|
|
else
|
|
{
|
|
AB_BC_CA_To_ABC(v->vars.Uab,v->vars.Ubc,v->vars.Uca, &v->vars.Ua, &v->vars.Ub, &v->vars.Uc);
|
|
}
|
|
#ifdef ROTATION_U_CBA
|
|
|
|
#else
|
|
|
|
#endif
|
|
|
|
v->vars.sum_zeroU_A_B_C = v->vars.Ua + v->vars.Ub + v->vars.Uc;
|
|
|
|
abc_to_alphabeta_u_input.Ua = v->vars.Ua;
|
|
abc_to_alphabeta_u_input.Ub = v->vars.Ub;
|
|
abc_to_alphabeta_u_input.Uc = v->vars.Uc;
|
|
|
|
abc_to_alphabeta_u_input.calc(&abc_to_alphabeta_u_input);
|
|
|
|
|
|
v->vars.Ualpha = abc_to_alphabeta_u_input.Ualpha;
|
|
v->vars.Ubeta = abc_to_alphabeta_u_input.Ubeta;
|
|
|
|
|
|
alphabeta_to_dq_u_input.Ualpha = v->vars.Ualpha;
|
|
alphabeta_to_dq_u_input.Ubeta = v->vars.Ubeta;
|
|
alphabeta_to_dq_u_input.Tetta = v->vars.Tetta;
|
|
|
|
alphabeta_to_dq_u_input.calc(&alphabeta_to_dq_u_input);
|
|
|
|
|
|
v->vars.pll_Ud = alphabeta_to_dq_u_input.Ud;
|
|
v->vars.pll_Uq = alphabeta_to_dq_u_input.Uq;
|
|
|
|
|
|
// pidUdq.Fdb = v->pll_Ud;//.pll_Uq; //err = Ref - Fdb
|
|
// pidUdq.Ref = 0;
|
|
|
|
pidUdq.Ref = v->vars.pll_Uq; //err = Ref - Fdb
|
|
pidUdq.Fdb = 0;
|
|
|
|
pidUdq.calc(&pidUdq);
|
|
v->vars.wc = pidUdq.Out;
|
|
|
|
|
|
if (prev_flag_find_pll==0)
|
|
{
|
|
flag_reset_Tetta_p = 0;
|
|
}
|
|
|
|
if (v->output.flag_find_pll)
|
|
{
|
|
#ifdef USE_SMOOTH_FOR_CALC_WC
|
|
mysmooth.input = _IQtoIQ23(v->vars.wc);
|
|
mysmooth.add(&mysmooth);
|
|
mysmooth.calc(&mysmooth);
|
|
v->vars.w_shtrih = _IQ23toIQ(mysmooth.av);
|
|
#else
|
|
v->vars.w_shtrih = v->vars.wc;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
v->vars.w_shtrih = v->vars.wc;
|
|
}
|
|
|
|
|
|
v->vars.Tetta += v->vars.wc;
|
|
v->vars.Tetta = minus_plus_2_pi(v->vars.Tetta); // +- 2PI
|
|
|
|
if (v->output.flag_find_pll)
|
|
{
|
|
v->vars.dwc = v->vars.wc - v->vars.w_shtrih;
|
|
|
|
v->vars.Tetta_p += v->vars.dwc;
|
|
v->vars.Tetta_p = minus_plus_2_pi(v->vars.Tetta_p); // +- 2PI
|
|
|
|
v->vars.dTetta = v->vars.Tetta - v->vars.Tetta_p;// + iq_05Pi;
|
|
|
|
v->vars.dTetta = minus_plus_2_pi(v->vars.dTetta); // +- 2PI
|
|
|
|
v->vars.Tetta_z = v->vars.dTetta;
|
|
|
|
// if (v->Tetta_z>=iq_05Pi && prev_Tetta_z<iq_05Pi)
|
|
// v->Tetta_p = 0;
|
|
|
|
Tetta_z_t = minus_plus_2_pi_v2(v->vars.Tetta_z);
|
|
|
|
if ( (Tetta_z_t>=0) && (Tetta_z_t<CONST_IQ_PI05) && ( (prev_Tetta_z>(CONST_IQ_2PI-CONST_IQ_PI05)) ) )
|
|
{
|
|
if (flag_reset_Tetta_p==0)
|
|
{
|
|
v->vars.Tetta_p = 0;
|
|
// flag_reset_Tetta_p = 1;
|
|
}
|
|
}
|
|
|
|
|
|
prev_Tetta_z = Tetta_z_t;
|
|
|
|
#ifdef USE_FILTER_TETTA
|
|
//use filter teta
|
|
v->vars.Tetta_v2 = v->vars.Tetta_z;//v->Tetta;
|
|
#else
|
|
//use mgnoven teta
|
|
v->vars.Tetta_v2 = v->vars.Tetta;
|
|
#endif
|
|
v->vars.delta_Tetta_c = v->vars.Tetta_z - v->vars.Tetta;
|
|
|
|
// prev_Tetta_v2 = v->Tetta_v2;
|
|
|
|
}
|
|
else
|
|
{
|
|
v->vars.Tetta_v2 = v->vars.Tetta;
|
|
flag_reset_Tetta_p = 0;
|
|
}
|
|
|
|
|
|
// PLL OK?
|
|
//count_wait_pll=0
|
|
//new alg find pll
|
|
if (v->vars.w_shtrih >= v->vars.find_min_w_strih && v->vars.w_shtrih <= v->vars.find_max_w_strih)
|
|
{
|
|
if (v->vars.count_wait_pll_w_shtrih < v->vars.max_time_wait_pll_w_strih)
|
|
v->vars.count_wait_pll_w_shtrih++;
|
|
}
|
|
else
|
|
{
|
|
if (v->vars.count_wait_pll_w_shtrih>0)
|
|
v->vars.count_wait_pll_w_shtrih--;
|
|
}
|
|
|
|
if (v->vars.count_wait_pll_w_shtrih == v->vars.max_time_wait_pll_w_strih)
|
|
v->output.flag_find_pll = 1;
|
|
|
|
if (v->vars.count_wait_pll_w_shtrih == 0)
|
|
v->output.flag_find_pll = 0;
|
|
|
|
//end new alg find pll
|
|
|
|
|
|
|
|
if ( (_IQabs(v->vars.pll_Uq)<=_IQabs(iqDetect_PLL_q)) // zero
|
|
&& (_IQabs(v->vars.pll_Ud)>=_IQabs(iqDetect_PLL_d) ) //ampl
|
|
)
|
|
{
|
|
count_err_pll=0;
|
|
if (count_wait_pll<MAX_TIME_WAIT_PLL)
|
|
count_wait_pll++;
|
|
else
|
|
// ok, pll finded
|
|
v->output.flag_find_pll = 1;
|
|
}
|
|
else
|
|
{
|
|
if (count_err_pll>=MAX_COUNT_ERR_PLL)
|
|
{
|
|
// fail find pll
|
|
count_wait_pll=0;
|
|
|
|
v->output.flag_find_pll = 0;
|
|
|
|
if (v->output.flag_find_pll==1)
|
|
{
|
|
v->vars.pll_Uq = 0;
|
|
v->vars.pll_Ud = 0;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
count_err_pll++;
|
|
if ((v->output.flag_find_pll==0) && (count_wait_pll>0))
|
|
count_wait_pll--;
|
|
}
|
|
}
|
|
|
|
// end PLL Ok?
|
|
|
|
|
|
v->vars.pi_teta_u_out = pidUdq.Out;
|
|
v->vars.pi_teta_u_i = pidUdq.Ui;
|
|
v->vars.pi_teta_u_p = pidUdq.Up;
|
|
|
|
|
|
prev_flag_find_pll = v->output.flag_find_pll;
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////
|
|
/////////////////////////////////////////////////
|
|
//#pragma CODE_SECTION(pll_get_freq,".fast_run");
|
|
/////////////////////////////////////////////////
|
|
void pll_get_freq_float(PLL_REC *v)
|
|
{
|
|
|
|
if (v->output.flag_find_pll)
|
|
{
|
|
v->output.int_freq_net = _IQtoF( v->vars.w_shtrih) * v->setup.freq_run_pll / PI * 50.00; // freq*100
|
|
}
|
|
else
|
|
{
|
|
v->output.int_freq_net = 0;
|
|
}
|
|
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////
|
|
void pll_get_freq_iq(PLL_REC *v)
|
|
{
|
|
|
|
if (v->output.flag_find_pll)
|
|
{
|
|
v->output.iq_freq_net = v->vars.w_shtrih;//_IQtoF( v->vars.w_shtrih) * v->setup.freq_run_pll / PI * 50.00; // freq*100
|
|
}
|
|
else
|
|
{
|
|
v->output.iq_freq_net = 0;
|
|
}
|
|
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
//#pragma CODE_SECTION(detect_phase_count,".fast_run");
|
|
void detect_phase_count(PLL_REC *v)
|
|
{
|
|
static _iq prev_Ua = 0;
|
|
static int prev_flag_find_pll=0;
|
|
|
|
|
|
|
|
// abc_to_dq.Ua
|
|
|
|
if ((v->output.flag_find_pll != prev_flag_find_pll) && (v->output.flag_find_pll == 1))
|
|
{
|
|
prev_Ua = v->vars.Ua;
|
|
v->vars.enable_detect_phase_count = 1;
|
|
v->vars.error_phase_count = 0;
|
|
}
|
|
|
|
|
|
if (v->output.flag_find_pll==0)
|
|
v->vars.enable_detect_phase_count = 0;
|
|
|
|
|
|
if (v->output.flag_find_pll)
|
|
{
|
|
if (v->vars.enable_detect_phase_count)
|
|
{
|
|
if ( (prev_Ua<0) && (v->vars.Ua>=0) )
|
|
{
|
|
|
|
if (v->vars.Uc > v->vars.Ub)
|
|
v->vars.enable_detect_phase_count = 0;
|
|
|
|
if (v->vars.Ub > v->vars.Uc)
|
|
{
|
|
v->vars.enable_detect_phase_count = 0;
|
|
v->vars.error_phase_count = 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
prev_flag_find_pll = v->output.flag_find_pll;
|
|
prev_Ua = v->vars.Ua;
|
|
|
|
}
|
|
/////////////////////////////////////////////////
|
|
|
|
|
|
////////////////////////////////////////////////
|
|
|
|
#pragma CODE_SECTION(Find_zero_Uabc,".fast_run");
|
|
void Find_zero_Uabc(PLL_REC *v)
|
|
{
|
|
static int c_a=0;
|
|
// static int c_b=0;
|
|
// static int c_c=0;
|
|
|
|
static _iq sum_a=0;
|
|
static _iq sum_b=0;
|
|
static _iq sum_c=0;
|
|
|
|
_iq22 sum_t,c_t;
|
|
|
|
_iq22 sum_div;
|
|
|
|
|
|
// AB_BC_CA_To_ABC(analog.iqUin_AB-iqZeroUABC, analog.iqUin_BC-iqZeroUABC, analog.iqUin_CA-iqZeroUABC);
|
|
|
|
|
|
sum_a += v->input.Input_U_AB; // analog.iqUin_AB;
|
|
sum_b += v->input.Input_U_BC; // analog.iqUin_BC;
|
|
sum_c += v->input.Input_U_CA; // analog.iqUin_CA;
|
|
|
|
c_a++;
|
|
|
|
if (c_a >= v->vars.count_sum_find_zero_uabc)
|
|
{
|
|
sum_div = v->vars.sum_div_find_zero_uabc;
|
|
|
|
sum_t = _IQtoIQ22(sum_a);
|
|
c_t = _IQ22div(sum_t,sum_div);
|
|
v->vars.iqZeroUAB = _IQ22toIQ(c_t);
|
|
|
|
sum_t = _IQtoIQ22(sum_b);
|
|
c_t = _IQ22div(sum_t,sum_div);
|
|
v->vars.iqZeroUBC = _IQ22toIQ(c_t);
|
|
|
|
sum_t = _IQtoIQ22(sum_c);
|
|
c_t = _IQ22div(sum_t,sum_div);
|
|
v->vars.iqZeroUCA = _IQ22toIQ(c_t);
|
|
|
|
sum_a = 0;
|
|
sum_b = 0;
|
|
sum_c = 0;
|
|
c_a = 0;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void pll_init(PLL_REC *v)
|
|
{
|
|
v->output.status = STATUS_PLL_NOT_INITED;
|
|
|
|
abc_to_alphabeta_u_input.Ua = 0;
|
|
abc_to_alphabeta_u_input.Ub = 0;
|
|
abc_to_alphabeta_u_input.Uc = 0;
|
|
abc_to_alphabeta_u_input.Ualpha = 0;
|
|
abc_to_alphabeta_u_input.Ubeta = 0;
|
|
|
|
alphabeta_to_dq_u_input.Tetta = 0;
|
|
alphabeta_to_dq_u_input.Ualpha = 0;
|
|
alphabeta_to_dq_u_input.Ubeta = 0;
|
|
alphabeta_to_dq_u_input.Ud = 0;
|
|
alphabeta_to_dq_u_input.Uq = 0;
|
|
|
|
v->vars.count_wait_pll_w_shtrih = 0;
|
|
|
|
v->vars.pll_Ud = 0;
|
|
v->vars.pll_Uq = 0;
|
|
v->vars.Tetta = 0;
|
|
v->vars.Tetta_p = 0;
|
|
|
|
v->vars.Ua = 0;
|
|
v->vars.Ub = 0;
|
|
v->vars.Uc = 0;
|
|
|
|
v->vars.Ualpha = 0;
|
|
v->vars.Ubeta = 0;
|
|
|
|
// count_wait_pll = 0;
|
|
count_err_pll = 0;
|
|
v->output.flag_find_pll = 0;
|
|
|
|
pidUdq.Kp = _IQ(v->setup.pid_kp_pll);
|
|
pidUdq.Ki = _IQ(v->setup.pid_ki_pll);
|
|
|
|
pidUdq.Kc = _IQ(PID_KC_PLL);
|
|
pidUdq.Kd = _IQ(0.0);
|
|
|
|
pidUdq.OutMax = _IQ(K_PLL_MAX);
|
|
pidUdq.OutMin = _IQ(K_PLL_MIN);
|
|
|
|
pidUdq.Err = 0;
|
|
pidUdq.Out = 0;
|
|
pidUdq.OutPreSat = 0;
|
|
pidUdq.SatErr = 0;
|
|
|
|
if (v->setup.freq_run_pll == 0)
|
|
v->setup.freq_run_pll = DEFAULT_FREQ_RUN_PLL;
|
|
|
|
pidUdq.Ui = _IQ(2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll));
|
|
|
|
// iqDetect_PLL_d = iqDetect_PLL_d;
|
|
// iqDetect_PLL_q = iqDetect_PLL_q;
|
|
|
|
#ifdef USE_SMOOTH_FOR_CALC_WC
|
|
mysmooth.init(&mysmooth);
|
|
#endif
|
|
|
|
|
|
v->vars.count_sum_find_zero_uabc = v->setup.freq_run_pll/DEFAULT_FREQ_NET; //79*2 //1975/50*2
|
|
v->vars.sum_div_find_zero_uabc = _IQ22(v->vars.count_sum_find_zero_uabc);
|
|
|
|
v->vars.find_max_w_strih = _IQ(FIND_MAX_W_STRIH*2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll));
|
|
v->vars.find_min_w_strih = _IQ(FIND_MIN_W_STRIH*2.0*PI*DEFAULT_FREQ_NET/(v->setup.freq_run_pll));
|
|
|
|
v->vars.max_time_wait_pll_w_strih = (v->vars.count_sum_find_zero_uabc * MAX_PERIOD_WAIT_PLL_W_SHTRIH);
|
|
|
|
v->output.status = STATUS_PLL_INITED;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#pragma CODE_SECTION(read_error_find_pll,".fast_run2");
|
|
int read_error_find_pll(PLL_REC *v)
|
|
{
|
|
// static int enable_detect_pll_err=0;
|
|
// static int prev_flag_find_pll=0;
|
|
static int err_pll=0;
|
|
|
|
|
|
err_pll = 0;
|
|
/*
|
|
if ((v->output.flag_find_pll!=prev_flag_find_pll)
|
|
&& (v->output.flag_find_pll==1))
|
|
{
|
|
enable_detect_pll_err = 1;
|
|
}
|
|
prev_flag_find_pll = v->output.flag_find_pll;
|
|
|
|
if (v->input.enable_find_pll==0)
|
|
enable_detect_pll_err = 0;
|
|
|
|
if ((enable_detect_pll_err) && (v->output.flag_find_pll==0) && (v->input.enable_find_pll==1))
|
|
{
|
|
err_pll = 1;
|
|
}
|
|
*/
|
|
return err_pll;
|
|
}
|
|
|
|
|
|
|
|
|
|
#pragma CODE_SECTION(pll_calc,".fast_run");
|
|
void pll_calc(PLL_REC *v)
|
|
{
|
|
if (v->output.status >= STATUS_PLL_INITED)
|
|
{
|
|
Find_zero_Uabc(v);
|
|
PLLController(v);
|
|
// detect_phase_count(v);
|
|
v->output.status = STATUS_PLL_OK;
|
|
}
|
|
|
|
}
|