今天繼續定點的工作，定點上使用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的相位突變引起的不穩定情況。
本人能力有限，正在學習定點編程，如有錯誤懇請幫忙指正，謝謝。