📄 pid_i.asm
字号:
;============================================================
; Filename: pid_i.asm
;
; Module names: pid_reg_id, pid_reg_iq
;
; Initialization routines: pid_reg_id_init, pid_reg_iq_init
;
; Originator: Digital control systems Group
; Texas Instruments
;
; Description: PI current regulator with integral correction
; for d and q axes
;
; |~~~~~~~~~~~~|
; i_fdb o------>| |
; i_ref o------>| pid_reg |----->o u_out
; u_int o------>| |
; Kp | |
; Ki o------>|____________|
; Kc
;
; Target dependency: 'c2xx core only
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000 Release Rev 1.0
;====================================================================
*************************************************************
* D-Axis PI Current Regulator
*************************************************************
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
; .ref pid_reg_id,pid_reg_id_init ; function call
; .ref id_fdb,id_ref,Kp_d,Ki_d,Kc_d ; Inputs
; .ref ud_int ; Input
; .ref ud_out ; Outputs
;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
.def pid_reg_id,pid_reg_id_init ; function call
.def id_fdb,id_ref,Kp_d,Ki_d,Kc_d ; Inputs
.def ud_int ; Input
.def ud_out ; Outputs
;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------
id_fdb .usect "pid",1 ; current feedback
id_ref .usect "pid",1 ; current reference
ud_out .usect "pid",1 ; control voltage output
ud_int .usect "pid",1 ; error integral
uintlo_d .usect "pid",1
Kp_d .usect "pid",1 ; proportional gain
Ki_d .usect "pid",1 ; integral gain
Kc_d .usect "pid",1 ; integral correction gain
id_error .usect "pid",1 ; current error
uprsat_d .usect "pid",1 ; control voltage prio saturation
saterr_d .usect "pid",1 ; saturation error
tmp_d .usect "pid",1 ; temp scrach
;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------
Kp_d_ .set 388 ; Q11, proportional gain
Ki_d_ .set 1271 ; Q25, integral gain
Kc_d_ .set 32767 ; Q14, saturation correction gain
Umax_d_ .set 07000h ; maximum U
Umin_d_ .set 08FFFh ; minimum U
;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
pid_reg_id_init
ldp #Kp_d ;
SPLK #Kp_d_,Kp_d ;
SPLK #Ki_d_,Ki_d ;
SPLK #Kc_d_,Kc_d ;
SPLK #0,ud_int ; zero integral term
SPLK #0,uintlo_d
RET
;------------------------------------------------------------
; Routine
;------------------------------------------------------------
pid_reg_id
setc SXM ; Allow sign extension
setc OVM ; Set overflow protection mode
ldp #id_ref ;
LACC id_ref,16 ; Use ACCH for OV protection
SUB id_fdb,16 ;
SACH id_error ; Q15 id_ref - id_fdb
lacl uintlo_d ;
add ud_int,16 ; 32-bit Q30
spm 2 ; product l/s 4 for accumulation
LT id_error ;
mpy Kp_d ; Q15*Q11 -> 32-bit Q26
apac ; 32-bit Q30 uint + id_error*Kp_d
SACH uprsat_d ; save as Q14 uprsat_d
sacl tmp_d ;
adds tmp_d ;
add uprsat_d,16 ; Q30 -> Q31 with OV protection
sach tmp_d ; save to tmp_d as Q15
lacc tmp_d ;
sub #Umin_d_ ;
bcnd U_gmind,GEQ ; Continue if tmp_d>=U_min
lacc #Umin_d_ ; otherwise, saturate
B Nextd
U_gmind
lacc tmp_d ;
sub #Umax_d_ ;
BCND U_lmaxd,LEQ ; Continue if tmp_d<=U_max
lacc #Umax_d_ ; otherwise, saturate
b Nextd
U_lmaxd
lacc tmp_d ;
Nextd
sacl ud_out ;
Int_termd
lacc ud_out,15 ; Use ACCH for OV protection
SUB uprsat_d,16 ;
sach saterr_d ; save as Q14 saterr_d = ud_out-uprsat_d
lt id_error ;
mpy Ki_d ; Q15*Q25 -> Q40
pac ; Q40 -> Q44
sach tmp_d ;
lacc tmp_d ; Q44 -> Q28 (r/s 16 bits)
LT saterr_d ;
MPY Kc_d ; Q14*Q14 -> Q28 saterr_d * Kc_d
APAC ; Q28 Ki_d*id_error + Kc_d*saterr_d
norm * ;
norm * ; Q28 -> Q30 (with OV protection)
ADDS uintlo_d
ADD ud_int,16 ; uint + saterr_d*Kc_d + id_error*Ki_d
SACH ud_int
SACL uintlo_d ; save as 32-bit Q30
RET
***END D-Axis PI Current Regulator
*************************************************************
* Q-Axis PI Current Regulator
*************************************************************
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
; .ref pid_reg_iq,pid_reg_iq_init ; function call
; .ref iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q ; Inputs
; .ref uq_int ; Input
; .ref uq_out ; Outputs
;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
.def pid_reg_iq,pid_reg_iq_init ; function call
.def iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q ; Inputs
.def uq_int ; Input
.def uq_out ; Outputs
;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------
iq_fdb .usect "pid",1 ; current feedback
iq_ref .usect "pid",1 ; current reference
uq_out .usect "pid",1 ; control voltage output
uq_int .usect "pid",1 ; error integral
uintlo_q .usect "pid",1
Kp_q .usect "pid",1 ; proportional gain
Ki_q .usect "pid",1 ; integral gain
Kc_q .usect "pid",1 ; integral correction gain
iq_error .usect "pid",1 ; current error
uprsat_q .usect "pid",1 ; control voltage prio saturation
saterr_q .usect "pid",1 ; saturation error
tmp_q .usect "pid",1 ; temp scrach
;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------
Kp_q_ .set 776 ; Q11, proportional gain
Ki_q_ .set 1907 ; Q25, integral gain
Kc_q_ .set 24576 ; Q14, saturation correction gain
Umax_q_ .set 07000h ; maximum U
Umin_q_ .set 08FFFh ; minimum U
;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
pid_reg_iq_init
ldp #Kp_q ;
SPLK #Kp_q_,Kp_q ;
SPLK #Ki_q_,Ki_q ;
SPLK #Kc_q_,Kc_q ;
SPLK #0,uq_int ; zero integral term
SPLK #0,uintlo_q
RET
;------------------------------------------------------------
; Routine
;------------------------------------------------------------
pid_reg_iq
setc SXM ; Allow sign extension
setc OVM ; Set overflow protection mode
ldp #iq_ref ;
LACC iq_ref,16 ; Use ACCH for OV protection
SUB iq_fdb,16 ;
SACH iq_error ; Q15 iq_ref - iq_fdb
lacl uintlo_q ;
add uq_int,16 ; 32-bit Q30
spm 2 ; product l/s 4 before accumulation
LT iq_error ;
mpy Kp_q ; Q15*Q11 -> 32-bit Q26
apac ; 32-bit Q30 uint + iq_error*Kp_q
SACH uprsat_q ; save as Q14 uprsat_q
sacl tmp_q ;
adds tmp_q ;
add uprsat_q,16 ; Q30 -> Q31 with OV protection
sach tmp_q ; save to tmp_q as Q15
lacc tmp_q ;
sub #Umin_q_ ;
bcnd U_gminq,GEQ ; Continue if tmp_q>=U_min
lacc #Umin_q_ ; otherwise, saturate
B Nextq
U_gminq
lacc tmp_q ;
sub #Umax_q_ ;
BCND U_lmaxq,LEQ ; Continue if tmp_q<=U_max
lacc #Umax_q_ ; otherwise, saturate
b Nextq
U_lmaxq
lacc tmp_q ;
Nextq
sacl uq_out ;
Int_termq
lacc uq_out,15 ; Use ACCH for OV protection
SUB uprsat_q,16 ;
sach saterr_q ; save as Q14 saterr_q = uq_out-uprsat_q
lt iq_error ;
mpy Ki_q ; Q15*Q26 -> Q40
pac ; Q40 -> Q44
sach tmp_q ;
lacc tmp_q ; Q44 -> Q28 (r/s 16 bits)
LT saterr_q ;
MPY Kc_q ; Q14*Q14 -> Q28 saterr_q * Kc_q
APAC ; Q28 Ki_q*iq_error + Kc_q*saterr_q
norm * ;
norm * ; Q28 -> Q30 (with OV protection)
ADDS uintlo_q
ADD uq_int,16 ; uint + saterr_q*Kc_q + iq_error*Ki_q
SACH uq_int
SACL uintlo_q ; save as 32-bit Q30
RET
***END Q-Axis PI Current Regulator
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -