📄 aci_fe.asm
字号:
;=======================================================================
; File Name: aci_fe.asm
;
; Module Name: ACI_FE
;
; Initialization Routine: ACI_FE_INIT
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description: Flux Estimator of Induction Motor
;
;
; |~~~~~~~~~~~~~~~~~~~~|
; u_ds_fe o----->|Q15 Q15|----->o psi_dr_fe
; u_qs_fe o----->|Q15 ACI_FE Q15|----->o psi_qr_fe
; i_ds_fe o----->|Q15 Q15|----->o theta_r_fe
; i_qs_fe o----->|Q15 |
; |____________________|
;
; Note 0000h < theta_r_fe < 7FFFh
; (i.e. equivalent to 0 < theta_r_fe < 360 deg)
;
; Target dependency: C2xx core only
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 11-15-2000 Release Rev 1.00
; 12-15-2005 Release Rev 3.11: Change the sign in current model equation
;===========================================================================
;(To use this Module, copy this section to main system file)
; .ref ACI_FE, ACI_FE_INIT ;function call
; .ref u_ds_fe, u_qs_fe ;Inputs
; .ref i_ds_fe, i_qs_fe ;Inputs
; .ref psi_dr_fe, psi_qr_fe ;Outputs
; .ref theta_r_fe ;Outputs
;===========================================================================
;Module definitions for external reference.
.def ACI_FE, ACI_FE_INIT ;function call
.def u_ds_fe, u_qs_fe ;Inputs
.def i_ds_fe, i_qs_fe ;Inputs
.def psi_dr_fe, psi_qr_fe ;Outputs
.def theta_r_fe ;Outputs
;===========================================================================
.ref SINTAB_360
.ref ATANTAB_45
motor1 .set 0 ; 1-Hp motor (WEG)
motor2 .set 0 ; 1-Hp motor (Dayton)
motor3 .set 1 ; 1/4-Hp motor (Dayton)
.if (motor1)
; original parameters
K1_fe_ .set 32749 ; for K1_fe (Q15)
K2_fe_ .set 19 ; for K2_fe (Q15)
K3_fe_ .set 30881 ; for K3_fe (Q15)
K4_fe_ .set 3408 ; for K4_fe (Q15)
K5_fe_ .set 1824 ; for K5_fe (Q15)
K6_fe_ .set 318 ; for K6_fe (Q15)
K7_fe_ .set 17385 ; for K7_fe (Q14)
K8_fe_ .set 3616 ; for K8_fe (Q15)
.endif
.if (motor2)
K1_fe_ .set 32754 ; for K1_fe (Q15)
K2_fe_ .set 14 ; for K2_fe (Q15)
K3_fe_ .set 31035 ; for K3_fe (Q15)
K4_fe_ .set 4394 ; for K4_fe (Q15)
K5_fe_ .set 2626 ; for K5_fe (Q15)
K6_fe_ .set 257 ; for K6_fe (Q15)
K7_fe_ .set 17299 ; for K7_fe (Q14)
K8_fe_ .set 4640 ; for K8_fe (Q15)
.endif
.if (motor3)
K1_fe_ .set 32739 ; for K1_fe (Q15)
K2_fe_ .set 29 ; for K2_fe (Q15)
K3_fe_ .set 31203 ; for K3_fe (Q15)
K4_fe_ .set 3209 ; for K4_fe (Q15)
K5_fe_ .set 11626 ; for K5_fe (Q15)
K6_fe_ .set 171 ; for K6_fe (Q15)
K7_fe_ .set 17206 ; for K7_fe (Q14)
K8_fe_ .set 3370 ; for K8_fe (Q15)
.endif
.include x24x_app.h
u_ds_fe .usect "fe_aci",1 ; Stationary d-axis stator voltage (Q15)
u_qs_fe .usect "fe_aci",1 ; Stationary q-axis stator voltage (Q15)
i_ds_fe .usect "fe_aci",1 ; Stationary d-axis stator current (Q15)
i_qs_fe .usect "fe_aci",1 ; Stationary q-axis stator current (Q15)
psi_dr_fe .usect "fe_aci",1 ; Stationary d-axis estimated rotor flux (Q15)
psi_qr_fe .usect "fe_aci",1 ; Stationary q-axis estimated rotor flux (Q15)
theta_r_fe .usect "fe_aci",1 ; Rotor flux angle (Q15)
K1_fe .usect "fe_aci",1 ; Constant using in current model (Q15)
K2_fe .usect "fe_aci",1 ; Constant using in current model (Q15)
K3_fe .usect "fe_aci",1 ; Constant using in stator flux computation (Q15)
K4_fe .usect "fe_aci",1 ; Constant using in stator flux computation (Q15)
K5_fe .usect "fe_aci",1 ; Constant using in back emf computation (Q15)
K6_fe .usect "fe_aci",1 ; Constant using in integral back emf (Q15)
K7_fe .usect "fe_aci",1 ; Constant using in rotor flux computation (Q14)
K8_fe .usect "fe_aci",1 ; Constant using in rotor flux computation (Q15)
K9_fe .usect "fe_aci",1 ; Constant using in torque computation (Q12)
Kp_fe .usect "fe_aci",1 ; PI proportionnal constant (Q15)
Ki_fe .usect "fe_aci",1 ; PI integral constant (Q31-16 bit)
ptr_fe .usect "fe_aci",1 ; Table pointer for sin, cos, and atan
cos_fe .usect "fe_aci",1 ; Cos(theta_r_fe) (Q15)
sin_fe .usect "fe_aci",1 ; Sin(theta_r_fe) (Q15)
i_ds_e .usect "fe_aci",1 ; Rotating d-axis stator voltage (Q15)
i_qs_e .usect "fe_aci",1 ; Rotating q-axis stator voltage (Q15)
flx_dr_e .usect "fe_aci",1 ; Rotating d-axis rotor flux (current model) (Q15)
flx_qr_s .usect "fe_aci",1 ; Stationary q-axis rotor flux (current model) (Q15)
flx_dr_s .usect "fe_aci",1 ; Stationary d-axis rotor flux (current model) (Q15)
flx_qs_s .usect "fe_aci",1 ; Stationary q-axis stator flux (current model) (Q15)
flx_ds_s .usect "fe_aci",1 ; Stationary d-axis stator flux (current model) (Q15)
ucomp_qs .usect "fe_aci",1 ; Stationary q-axis compensated voltage (Q15)
ucomp_ds .usect "fe_aci",1 ; Stationary d-axis compensated voltage (Q15)
error_qs .usect "fe_aci",1 ; Stationary q-axis stator flux error (Q14)
error_ds .usect "fe_aci",1 ; Stationary d-axis stator flux error (Q14)
ui_lo_ds .usect "fe_aci",1 ; Stationary d-axis integral term (Q30)
ui_hi_ds .usect "fe_aci",1 ; Stationary d-axis integral term (Q30)
ui_lo_qs .usect "fe_aci",1 ; Stationary q-axis integral term (Q30)
ui_hi_qs .usect "fe_aci",1 ; Stationary q-axis integral term (Q30)
emf_qs .usect "fe_aci",1 ; Stationary q-axis back emf (Q15)
emf_ds .usect "fe_aci",1 ; Stationary d-axis back emf (Q15)
psi_qs_fe .usect "fe_aci",1 ; Stationary q-axis stator flux (Q31)
psi_ds_fe .usect "fe_aci",1 ; Stationary d-axis stator flux (Q31)
psi_qs_lo .usect "fe_aci",1 ; Stationary q-axis stator flux (Q31)
psi_ds_lo .usect "fe_aci",1 ; Stationary q-axis stator flux (Q31)
psi_qr_p .usect "fe_aci",1 ; Stationary q-axis positive rotor flux (Q15)
psi_dr_p .usect "fe_aci",1 ; Stationary d-axis positive rotor flux (Q15)
psi_qdr .usect "fe_aci",1 ; Division result of rotor flux (Q15)
tmp1_fe .usect "fe_aci",1 ; Temporary variable
;=========================
ACI_FE_INIT:
;=========================
LDP #K1_fe
SPLK #K1_fe_,K1_fe ; K1 = Tr/(Tr+T) (Q15)
SPLK #K2_fe_,K2_fe ; K2 = T/(Tr+T) (Q15)
SPLK #K3_fe_,K3_fe ; K3 = Lm/Lr (Q15)
SPLK #K4_fe_,K4_fe ; K4 = (Ls*Lr-Lm*Lm)/(Lr*Lm) (Q15)
SPLK #K5_fe_,K5_fe ; K5 = Rs*Ib/Vb (Q15)
SPLK #K6_fe_,K6_fe ; K6 = T*Vb/(Lm*Ib) (Q15)
SPLK #K7_fe_,K7_fe ; K7 = Lr/Lm (Q14)
SPLK #K8_fe_,K8_fe ; K8 = (Ls*Lr-Lm*Lm)/(Lm*Lm) (Q15)
SPLK #1000h,Kp_fe ; PI proportionnal constant (Q15)
SPLK #0B00h,Ki_fe ; PI integral constant (Q31-16 bit)
SPLK #0h,flx_dr_e ; Initialize the rotor flux (Q15)
SPLK #0h,error_qs ; Initialize the stator flux error (Q14)
SPLK #0h,error_ds ; Initialize the stator flux error (Q14)
SPLK #0h,ucomp_qs ; Initialize the compensated voltage (Q15)
SPLK #0h,ucomp_ds ; Initialize the compensated voltage (Q15)
SPLK #0h,emf_qs ; Initialize the back emf (Q15)
SPLK #0h,emf_ds ; Initialize the back emf (Q15)
SPLK #0h,psi_qs_fe ; Initialize the stator flux (Q31)
SPLK #0h,psi_ds_fe ; Initialize the stator flux (Q31)
SPLK #0h,psi_qs_lo ; Initialize the stator flux (Q31)
SPLK #0h,psi_ds_lo ; Initialize the stator flux (Q31)
SPLK #0h,ui_lo_ds ; Initialize the integral term (Q30)
SPLK #0h,ui_hi_ds ; Initialize the integral term (Q30)
SPLK #0h,ui_lo_qs ; Initialize the integral term (Q30)
SPLK #0h,ui_hi_qs ; Initialize the integral term (Q30)
RET
;=========================
ACI_FE:
;=========================
SPM 0 ; Reset product mode
SETC SXM ; Set sign extension mode
SETC OVM ; Set overflow mode
LDP #theta_r_fe
; Park transformation on the measured stator current
LACC theta_r_fe ; ACC = theta_r_fe (Q15)
ADD #8192 ; ACC = theta_r_fe + 90 deg (ie,COS(A)=SIN(A+90))
AND #07FFFh ; Force positive wrap-around
SACL tmp1_fe ; tmp1_fe = 90 deg = 7FFFh/4
LACC tmp1_fe,9
SACH ptr_fe
LACC #SINTAB_360
ADD ptr_fe
TBLR cos_fe ; cos_fe = cos(theta_r_fe) (Q15)
LACC theta_r_fe,9
SACH ptr_fe
LACC #SINTAB_360
ADD ptr_fe
TBLR sin_fe ; sin_fe = sin(theta_r_fe) (Q15)
;***** d-axis *****
SPM 1 ; SPM set for Q15 multiplication
ZAC ; Reset accumulator
LT i_qs_fe ; TREG = i_qs_fe
MPY sin_fe ; PREG = i_qs_fe*sin(theta_r_fe)
LTA i_ds_fe ; ACC = i_qs_fe*sin(theta_r_fe) and TREG = i_ds_fe
MPY cos_fe ; PREG = i_ds_fe*cos(theta_r_fe)
MPYA sin_fe ; ACC = i_qs_fe*sin(theta_r_fe)+i_ds_fe*cos(theta_r_fe)
; PREG = i_ds_fe*sin(theta_r_fe)
SACH i_ds_e ; i_ds_e = i_qs_fe*sin(theta_r_fe)+i_ds_fe*cos(theta_r_fe)
;***** q-axis *****
LACC #0 ; Clear ACC
LT i_qs_fe ; TREG = i_qs_fe
MPYS cos_fe ; ACC = -i_ds_fe*sin(theta_r_fe) and PREG = i_qs_fe*cos(theta_r_fe)
APAC ; ACC = i_qs_fe*cos(theta_r_fe)-i_ds_fe*sin(theta_r_fe)
SACH i_qs_e ; i_qs_e = i_qs_fe*cos(theta_r_fe)-i_ds_fe*sin(theta_r_fe)
; The current model section (Classical Rotor Flux Vector Control Equation)
; flx_dr_e(k) = K1*flx_dr_e(k-1)+K2*ids_e(k) => Q15 = Q15*Q15+Q15*Q15
; flx_qr_e(k) = 0
SPM 0 ; Reset product mode
LT K1_fe ; TREG = K1 (Q15)
MPY flx_dr_e ; PREG = K1*flx_dr_e(k-1) (Q30)
PAC ; ACC = K1*flx_dr_e(k-1) (Q30)
LT K2_fe ; TREG = K2 (Q15)
MPY i_ds_e ; PREG = ids_e(k) (Q30)
APAC ; ACC = K1*flx_dr_e(k-1)+K2*ids_e(k) (Q30)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -