今天继续定点的工作,定点上使用PR控制还是有很意义的,毕竟浮点单片机很贵。
以前在浮点的开发可见:准比例谐振控制器(QPR)的离散化与代码实现
定点实现:考虑到量化误差,直接把PR的IIR2函数运行在IQ26上,这样比单精度浮点精度提升4倍,整数还有最大32的数字,这样足够满足PR控制的输出动态范围,实在不行还可以把PR的输出在直接扩大2^N倍来满足控制器的调制范围。
IIR2定义:
typedef struct IIR_2ORDER_IQ_TAG{
Uint16 coeff_init_flag;
_iq filter_out;
_iq filter_W0;
_iq filter_W1;
_iq filter_W2;
_iq coeff_B0;
_iq coeff_B1;
_iq coeff_B2;
_iq coeff_A1;
_iq coeff_A2;
}IIR_2ORD_IQ_DATA_DEF;
函数:
static inline _iq iir2_IQ_func_(volatile _iq input, volatile IIR_2ORD_IQ_DATA_DEF *p)
{
//
// w0 = x(0) - A1 * W1 - A2 * W2
//
p->filter_W0 = input - _IQ26mpy(p->coeff_A1, p->filter_W1) - _IQ26mpy(p->coeff_A2, p->filter_W2);
//
// Y(0) = Gain * (B0 * W0 + B1 * W1 + B2 * W2)
//
p->filter_out = _IQ26mpy(p->coeff_B0, p->filter_W0) + _IQ26mpy(p->coeff_B1, p->filter_W1) + _IQ26mpy(p->coeff_B2, p->filter_W2);
//
// Update last data
//
p->filter_W2 = p->filter_W1;
p->filter_W1 = p->filter_W0;
return(p->filter_out);
}
PR控制系数计算,输入直接使用浮点数字,这样调试参数毕竟安逸,IQ26的系数计算在该函数里面进行:
static inline void updata_pr_ctrl_coeff_IQ( volatile IIR_2ORD_IQ_DATA_DEF *p,
float32 kr,
float32 wc,
float32 wr,
float32 kp,
float32 ts)
{
// with KP gain
float32 ts_x_ts = ts * ts;
float32 wr_x_wr = wr * wr;
float32 div_x = ts * ts * wr * wr + 4.0f * wc * ts + 4.0f;
#if 1
p->coeff_B0 = _IQ26((4.0f * kp + kp * ts_x_ts * wr_x_wr + 4.0f * kp * ts * wc + 4.0f * kr * wc * ts)/div_x);
p->coeff_B1 = _IQ26(-1.0f * (8.0f * kp - 2.0f * kp * ts_x_ts * wr_x_wr)/div_x);
p->coeff_B2 = _IQ26((4.0f * kp + kp * ts_x_ts * wr_x_wr - 4.0f * kp * ts * wc - 4.0f * kr * ts * wc)/div_x);
p->coeff_A1 = _IQ26((2.0f * ts_x_ts * wr_x_wr - 8.0f)/div_x);
p->coeff_A2 = _IQ26((ts_x_ts * wr_x_wr - 4.0f * ts * wc + 4.0f)/div_x);
#else
// without KP gain
float32 coeff_B0 = (4.0f * kr * wc * ts)/div_x;
float32 coeff_B1 = 0;
float32 coeff_B2 = -1.0f * p->coeff_B0;
float32 coeff_A1 = (2.0f * ts_x_ts * wr_x_wr - 8.0f)/div_x;
float32 coeff_A2 = (ts_x_ts * wr_x_wr - 4.0f * ts * wc + 4.0f)/div_x;
p->coeff_B0 = _IQ(coeff_B0);
p->coeff_B1 = _IQ(coeff_B1);
p->coeff_B2 = _IQ(coeff_B2);
p->coeff_A1 = _IQ(coeff_A1);
p->coeff_A2 = _IQ(coeff_A2);
#endif
}
PR数据结构定义
typedef struct PR_CTRL_LAW_DATA_IQ_TAG{
IIR_2ORD_IQ_DATA_DEF f_1st;
IIR_2ORD_IQ_DATA_DEF f_3st;
IIR_2ORD_IQ_DATA_DEF f_5st;
IIR_2ORD_IQ_DATA_DEF lead_lag_comp;
_iq f1stOut;
_iq f3rdOut;
_iq f5thOut;
_iq max;
_iq min;
_iq pr_ctrl_out;
}PR_CTRL_IQ_DATA_DEF;
PR+超前滞后补偿:
static inline _iq pr_ctrl_IQ_func( _iq error, PR_CTRL_IQ_DATA_DEF *p)
{
const _iq error_iq26 = _IQmpy4(error);
p->f1stOut = iir2_IQ_func_(error_iq26, &p->f_1st);
p->f3rdOut = iir2_IQ_func_(error_iq26, &p->f_3st);
p->f5thOut = iir2_IQ_func_(error_iq26, &p->f_5st);
p->pr_ctrl_out = (p->f1stOut + p->f3rdOut + p->f5thOut);
p->pr_ctrl_out = iir2_IQ_func_(p->pr_ctrl_out, &p->lead_lag_comp);
_IQsat(p->pr_ctrl_out, p->max, p->min);
return(_IQdiv4(p->pr_ctrl_out));
}
PR控制函数整体参数计算:
static void pr_ctrl_coeff_IQ_func(
PR_CTRL_IQ_DATA_DEF *p,
float32 grid_freq,
float32 kp,
float32 kr,
float32 ts,
float32 lead_fz,
float32 lead_fp,
float32 max,
float32 min)
{
updata_pr_ctrl_coeff_IQ(&p->f_1st, kr, 2.0f * M_PI, 2.0f * M_PI * grid_freq, kp, ts);
updata_pr_ctrl_coeff_IQ(&p->f_3st, kr, 2.0f * M_PI, 2.0f * M_PI * grid_freq * 3.0f, kp, ts);
updata_pr_ctrl_coeff_IQ(&p->f_5st, kr, 2.0f * M_PI, 2.0f * M_PI * grid_freq * 5.0f, kp, ts);
float32 ts_x_wz = ts * 2.0f * M_PI * lead_fz;
float32 ts_x_wp_puls_2 = ts * 2.0f * M_PI * lead_fp + 2.0f;
p->lead_lag_comp.coeff_B0 = _IQ((ts_x_wz + 2.0f)/ts_x_wp_puls_2);
p->lead_lag_comp.coeff_B1 = _IQ((ts_x_wz - 2.0f)/ts_x_wp_puls_2);
p->lead_lag_comp.coeff_B2 = 0;
p->lead_lag_comp.coeff_A1 = _IQ((ts * 2.0f * M_PI * lead_fp - 2.0f)/ts_x_wp_puls_2);
p->lead_lag_comp.coeff_A2 = 0;
p->max = _IQ(max);
p->min = _IQ(min);
p->pr_ctrl_out = 0;
}
运行测试:
对比maltab:
PR函数离散化系数对比,上为matlab自带函数转换,下为上文编写的系数转换,对比DSP运行的结果,适应IQ26转换后,系数具有较高的精度。
适应PR控制135次再组合超前补偿器来补偿PR的相位突变引起的不稳定情况。
本人能力有限,正在学习定点编程,如有错误恳请帮忙指正,谢谢。