📄 aci_fe.asm
字号:
SACH flx_dr_e,1 ; flx_dr_e(k) = K1*flx_dr_e(k-1)+K2*ids_e(k) (Q15)
; Inverse park transformation on the rotor flux from the current model
;***** d-axis *****
LT flx_dr_e ; TREG = flx_dr_e (Q15)
MPY cos_fe ; PREG = flx_dr_e*cos(theta_r_fe) (Q30)
PAC ; ACC = flx_dr_e*cos(theta_r_fe) (Q30)
SACH flx_dr_s,1 ; flx_dr_s = flx_dr_e*cos(theta_r_fe) (Q15)
;***** q-axis *****
MPY sin_fe ; PREG = flx_dr_e*sin(theta_r_fe) (Q30)
PAC ; ACC = flx_dr_e*sin(theta_r_fe) (Q30)
SACH flx_qr_s,1 ; flx_qr_s = flx_dr_e*sin(theta_r_fe) (Q15)
; Compute the stator flux based on the rotor flux from current model
;***** stationary d-axis *****
LT K3_fe ; TREG = K3 (Q15)
MPY flx_dr_s ; PREG = K3*flx_dr_s (Q30)
PAC ; ACC = K3*flx_dr_s (Q30)
LT K4_fe ; TREG = K4 (Q15)
MPY i_ds_fe ; PREG = K4*i_ds_fe (Q30)
APAC ; ACC = K3*flx_dr_s + K4*i_ds_fe (Q30)
SACH flx_ds_s,1 ; flx_ds_s = K3*flx_dr_s + K4*i_ds_fe (Q15)
;***** stationary q-axis *****
LT K3_fe ; TREG = K3 (Q15)
MPY flx_qr_s ; PREG = K3*flx_qr_s (Q30)
PAC ; ACC = K3*flx_qr_s (Q30)
LT K4_fe ; TREG = K4 (Q15)
MPY i_qs_fe ; PREG = K4*i_qs_fte (Q30)
APAC ; ACC = K3*flx_qr_s + K4*i_qs_fe (Q30)
SACH flx_qs_s,1 ; flx_qs_s = K3*flx_qr_s + K4*i_qs_fe (Q15)
; Conventional PI controller section
;***** stationary d-axis *****
SPM 1 ; Set left shifted 1 bit
LACC psi_ds_fe,15 ; ACC = psi_ds_fe (Q14)
SUB flx_ds_s,15 ; ACC = psi_ds_fe - flx_ds_s (Q14)
SACH error_ds ; error_ds(k) = psi_ds_fe - flx_ds_s (Q14)
LT Kp_fe ; TREG = Kp (Q15)
MPY error_ds ; PREG = Kp*error_ds(k) (Q29)
PAC ; ACC = Kp*error_ds(k) (Q30)
SACH tmp1_fe ; tmp1 = Kp*error_ds(k) (Q14)
ADDS ui_lo_ds ; ACC = Kp*error_ds(k)+ui_ds(k-1) (Q30)
ADDH ui_hi_ds ; ACC = Kp*error_ds(k)+ui_ds(k-1) (Q30)
NORM * ; ACC = Kp*error_ds(k)+ui_ds(k-1) (Q31)
SACH ucomp_ds ; ucomp_ds(k) = Kp*error_ds(k)+ui_ds(k-1) (Q15)
SPM 3 ; Set right shifted 6 bit
LT tmp1_fe ; TREG = Kp*error_ds(k) (Q14)
MPY Ki_fe ; PREG = Kp*Ki*error_ds(k) (Q38)
PAC ; ACC = Kp*Ki*error_ds(k) (Q32)
SFR ; ACC = Kp*Ki*error_ds(k) (Q31)
SFR ; ACC = Kp*Ki*error_ds(k) (Q30)
ADDS ui_lo_ds ; ACC = Kp*Ki*error_ds(k)+ui_ds(k-1) (Q30)
ADDH ui_hi_ds ; ACC = Kp*Ki*error_ds(k)+ui_ds(k-1) (Q30)
SACL ui_lo_ds ; ui_ds(k) = Kp*Ki*error_ds(k)+ui_ds(k-1) (Q30)
SACH ui_hi_ds ; ui_ds(k) = Kp*Ki*error_ds(k)+ui_ds(k-1) (Q30)
;***** stationary q-axis *****
SPM 1 ; Set left shifted 1 bit
LACC psi_qs_fe,15 ; ACC = psi_qs_fe (Q14)
SUB flx_qs_s,15 ; ACC = psi_qs_fe - flx_qs_s (Q14)
SACH error_qs ; error_qs(k) = psi_qs_fe - flx_qs_s (Q14)
LT Kp_fe ; TREG = Kp (Q15)
MPY error_qs ; PREG = Kp*error_qs(k) (Q29)
PAC ; ACC = Kp*error_qs(k) (Q30)
SACH tmp1_fe ; tmp1 = Kp*error_qs(k) (Q14)
ADDS ui_lo_qs ; ACC = Kp*error_qs(k)+ui_qs(k-1) (Q30)
ADDH ui_hi_qs ; ACC = Kp*error_qs(k)+ui_qs(k-1) (Q30)
NORM * ; ACC = Kp*error_qs(k)+ui_qs(k-1) (Q31)
SACH ucomp_qs ; ucomp_qs(k) = Kp*error_qs(k)+ui_qs(k-1) (Q15)
SPM 3 ; Set right shifted 6 bit
LT tmp1_fe ; TREG = Kp*error_qs(k) (Q14)
MPY Ki_fe ; PREG = Kp*Ki_fte*error_qs(k) (Q38)
PAC ; ACC = Kp*Ki*error_qs(k) (Q32)
SFR ; ACC = Kp*Ki*error_qs(k) (Q31)
SFR ; ACC = Kp*Ki*error_qs(k) (Q30)
ADDS ui_lo_qs ; ACC = Kp*Ki*error_qs(k)+ui_qs(k-1) (Q30)
ADDH ui_hi_qs ; ACC = Kp*Ki*error_qs(k)+ui_qs(k-1) (Q30)
SACL ui_lo_qs ; ui_qs(k) = Kp*Ki*error_qs(k)+ui_qs(k-1) (Q30)
SACH ui_hi_qs ; ui_qs(k) = Kp*Ki*error_qs(k)+ui_qs(k-1) (Q30)
; Compute the estimated stator flux based on the integral of back emf
;***** stationary d-axis *****
SPM 1 ; Set left shifted 1 bit
LACC emf_ds ; ACC = emf_ds(k-1) (Q15)
SACL tmp1_fe ; tmp1_fe = emf_ds(k-1) (Q15)
LACC u_ds_fe,16 ; ACC = u_ds_fe(k) (Q31)
SUB ucomp_ds,16 ; ACC = u_ds_fe(k)-ucomp_ds(k) (Q31)
LT K5_fe ; TREG = K5 (Q15)
MPY i_ds_fe ; PREG = K5*i_ds_fe (Q30)
SPAC ; ACC = u_ds_fe(k)-ucomp_ds(k)-K5*i_ds_fe (Q31)
SACH emf_ds ; emf_ds(k) = u_ds_fe(k)-ucomp_ds(k)-K5*i_ds_fe (Q15)
LACC emf_ds,15 ; ACC = emf_ds(k)/2 (Q15)
ADD tmp1_fe,15 ; ACC = emf_ds(k)/2+emf_ds(k-1)/2 (Q15)
SACH tmp1_fe ; tmp1_fe = emf_ds(k)/2+emf_ds(k-1)/2 (Q15)
LT K6_fe ; TREG = K6 (Q15)
MPY tmp1_fe ; PREG = K6*(1/2)*(emf_ds(k)+emf_ds(k-1)) (Q30)
PAC ; ACC = K6*(1/2)*(emf_ds(k)+emf_ds(k-1)) (Q31)
ADDS psi_ds_lo ; ACC = psi_ds_fe(k-1)+K6*(1/2)*(emf_ds(k)+emf_ds(k-1)) (Q31)
ADDH psi_ds_fe ; ACC = psi_ds_fe(k-1)+K6*(1/2)*(emf_ds(k)+emf_ds(k-1)) (Q31)
SACL psi_ds_lo ; psi_ds_fe(k) = psi_ds_fe(k-1)+K6*(1/2)*(emf_ds(k)+emf_ds(k-1)) (Q31)
SACH psi_ds_fe ; psi_ds_fe(k) = psi_ds_fe(k-1)+K6*(1/2)*(emf_ds(k)+emf_ds(k-1)) (Q31)
;***** stationary q-axis *****
LACC emf_qs ; ACC = emf_qs(k-1) (Q15)
SACL tmp1_fe ; tmp1_fte = emf_qs(k-1) (Q15)
LACC u_qs_fe,16 ; ACC = u_qs_fe(k) (Q31)
SUB ucomp_qs,16 ; ACC = u_qs_fe(k)-ucomp_qs(k) (Q31)
LT K5_fe ; TREG = K5 (Q15)
MPY i_qs_fe ; PREG = K5*i_qs_fe (Q30)
SPAC ; ACC = u_qs_fe(k)-ucomp_qs(k)-K5*i_qs_fe (Q31)
SACH emf_qs ; emf_qs(k) = u_qs_fe(k)-ucomp_qs(k)-K5*i_qs_fe (Q15)
LACC emf_qs,15 ; ACC = emf_qs(k)/2 (Q15)
ADD tmp1_fe,15 ; ACC = emf_qs(k)/2+emf_qs(k-1)/2 (Q15)
SACH tmp1_fe ; tmp1_fe = emf_qs(k)/2+emf_qs(k-1)/2 (Q15)
LT K6_fe ; TREG = K6 (Q15)
MPY tmp1_fe ; PREG = K6*(1/2)*(emf_qs(k)+emf_qs(k-1)) (Q30)
PAC ; ACC = K6*(1/2)*(emf_qs(k)+emf_qs(k-1)) (Q31)
ADDS psi_qs_lo ; ACC = psi_qs_fe(k-1)+K6*(1/2)*(emf_qs(k)+emf_qs(k-1)) (Q31)
ADDH psi_qs_fe ; ACC = psi_qs_fe(k-1)+K6*(1/2)*(emf_qs(k)+emf_qs(k-1)) (Q31)
SACL psi_qs_lo ; psi_qs_fe(k) = psi_qs_fe(k-1)+K6*(1/2)*(emf_qs(k)+emf_qs(k-1)) (Q31)
SACH psi_qs_fe ; psi_qs_fe(k) = psi_qs_fe(k-1)+K6*(1/2)*(emf_qs(k)+emf_qs(k-1)) (Q31)
; Compute the estimated rotor flux based on the stator flux from the integral of back emf
;***** stationary d-axis *****
SPM 0 ; Reset product mode
LT K8_fe ; TREG = K8 (Q15)
MPY i_ds_fe ; PREG = K8*i_ds_fe (Q30)
PAC ; ACC = K8*i_ds_fe (Q30)
NEG ; ACC = -K8*i_ds_fe (Q30)
SFR ; ACC = -K8*i_ds_fe (Q29)
LT K7_fe ; TREG = K7 (Q14)
MPY psi_ds_fe ; PREG = K7*psi_ds_fe (Q29)
APAC ; ACC = K7*psi_ds_fe-K8*i_ds_fe (Q29)
SACH psi_dr_fe,2 ; psi_dr_fe = K7*psi_ds_fe-K8*i_ds_fe (Q15)
;***** stationary q-axis *****
LT K8_fe ; TREG = K8 (Q15)
MPY i_qs_fe ; PREG = K8*i_qs_fe (Q30)
PAC ; ACC = K8*i_qs_fe (Q30)
NEG ; ACC = -K8*i_qs_fe (Q30)
SFR ; ACC = -K8*i_qs_fe (Q29)
LT K7_fe ; TREG = K7 (Q14)
MPY psi_qs_fe ; PREG = K7*psi_qs_fe (Q29)
APAC ; ACC = K7*psi_qs_fe-K8*i_qs_fe (Q29)
SACH psi_qr_fe,2 ; psi_qr_fe = K7*psi_qs_fe-K8*i_qs_fe (Q15)
; Compute the rotor flux angle
; Check |psi_qr_fe|/|psi_dr_fe|>1 ?
LACC psi_dr_fe ; ACC = psi_dr_fe
ABS ; ACC = |psi_dr_fe|
SACL psi_dr_p ; psi_dr_p = |psi_dr_fe|
LACC psi_qr_fe ; ACC = psi_qr_fe
ABS ; ACC = |psi_qr_fe|
SACL psi_qr_p ; psi_qr_p = |psi_qr_fe|
SUB psi_dr_p ; ACC = psi_qr_p - psi_dr_p
BCND LARGE_QD,GT ; Branch to LARGE_QD if psi_qr_p > psi_dr_p
BCND EQ_QD,EQ ; Branch to EQ_QD if psi_qr_p = psi_dr_p
; Here, |psi_qr_fe|/|psi_dr_fe| is less than 1.
SMALL_QD
LACC psi_qr_p,15 ; ACC = psi_qr_p left shifted by 15 (psi_qdr=Q15)
RPT #15 ; Repeat SUBC 16 times
SUBC psi_dr_p ; Dividing psi_qr_p/psi_dr_p
SACL psi_qdr ; psi_qdr = psi_qr_p/psi_dr_p (Q15)
; Looking at the table for arctan(|psi_qr_fe|/|psi_dr_fe|) directly
LACC psi_qdr,9 ; ACC = psi_qdr/128 (2^15/2^7 = 2^8 = 256)
SACH ptr_fe ; ptr_fte = psi_qdr/128
LACC #ATANTAB_45
ADD ptr_fe
TBLR theta_r_fe ; theta_r_fe = arctan(psi_qr_p/psi_dr_p) in Q15
B DIV_QD_END
; Here, |psi_qr_fe|/|psi_dr_fe| is greater than 1,
; so |psi_dr_fe|/|psi_qr_fe| < 1 is computed instead.
LARGE_QD
LACC psi_dr_p,15 ; ACC = psi_dr_p left shifted by 15 (psi_qdr=Q15)
RPT #15 ; Repeat SUBC 16 times
SUBC psi_qr_p ; Dividing psi_dr_p/psi_qr_p
SACL psi_qdr ; psi_qdr = psi_dr_p/psi_qr_p (Q15)
; Looking at the table for arctan(|psi_dr_fe|/|psi_qr_fe|)
LACC psi_qdr,9 ; ACC = psi_qdr/128 (2^15/2^7 = 2^8 = 256)
SACH ptr_fe ; atan_ptr = psi_qdr/128
LACC #ATANTAB_45
ADD ptr_fe
TBLR theta_r_fe ; theta_r_fe = arctan(psi_dr_p/psi_qr_p) in Q15
LACC #8192 ; ACC = 8192 (Q15) = 90 deg
SUB theta_r_fe ; ACC = 90 - arctan(psi_dr_p/psi_qr_p) (Q15)
SACL theta_r_fe ; theta_r_fe = arctan(psi_qr_p/psi_dr_p) (Q15)
B DIV_QD_END
; Here, |psi_qr_fe|/|psi_dr_fe| is equal to 1,
; i.e., |psi_qr_fe| = |psi_dr_fe|
EQ_QD
SPLK #4096,theta_r_fe ; theta_r_fe = 45 deg (Q15)
DIV_QD_END
; Determine the quadrant by looking at signs of psi_dr_fe and psi_qr_fe
LACC psi_dr_fe ; ACC = psi_dr_fe
BCND NEG_D, LT ; Branch to NEG_D if psi_dr_fe < 0
POS_D
LACC psi_qr_fe ; ACC = psi_qr_fe
BCND POS_D_NEG_Q, LT ; Branch to POS_D_NEG_Q if psi_qr_fe < 0
POS_D_POS_Q
; Do nothing here
B QUADRANT_QD_END
POS_D_NEG_Q
LACC #32767 ; ACC = 32767 = 360 deg
SUB theta_r_fe ; ACC = 360 - theta_r_fe (Q15)
SACL theta_r_fe ; theta_r_fe = 360 - theta_r_fe (Q15)
B QUADRANT_QD_END
NEG_D
LACC psi_qr_fe ; ACC = psi_qr_fe
BCND NEG_D_NEG_Q, LT ; Branch to NEG_D_NEG_Q if psi_qr_fe < 0
NEG_D_POS_Q
LACC #16384 ; ACC = 16384 = 180 deg
SUB theta_r_fe ; ACC = 180 - theta_r_fe (Q15)
SACL theta_r_fe ; theta_r_fe = 180 - theta_r_fe (Q15)
B QUADRANT_QD_END
NEG_D_NEG_Q
LACC #16384 ; ACC = 16384 = 180 deg
ADD theta_r_fe ; ACC = 180 + theta_r_fe (Q15)
SACL theta_r_fe ; theta_r_fe = 180 + theta_r_fe (Q15)
QUADRANT_QD_END
RET
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -