📄 aci_fe.asm
字号:
ADRK #15 ; ARP=AR2, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SACH *-,2 ; psi_qr_fe = K7_fe*psi_qs_fe-K8_fe*i_qs_fe (Q15)
; ARP=AR2, AR0->FR3, AR2->psi_dr_fe
;----------------------------------------------------------------------------------
; (8) Compute the rotor flux angle
;----------------------------------------------------------------------------------
; Checking |psi_qr_fe|/|psi_dr_fe| > 1 ?
LACC *+,AR0 ; ACC = psi_dr_fe (Q15)
; ARP=AR2, AR0->FR3, AR2->psi_qr_fe, ARP=AR0
;----------------------------------------------------------------------------------
ABS ; ACC = |psi_dr_fe| (Q15)
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SACL *+,AR2 ; FR3 = psi_dr_p = |psi_dr_fe| (Q15)
; ARP=AR0, AR0->FR4, AR2->psi_qr_fe, ARP=AR2
;----------------------------------------------------------------------------------
LACC *,AR0 ; ACC = psi_qr_fe (Q15)
; ARP=AR2, AR0->FR4, AR2->psi_qr_fe, ARP=AR0
;----------------------------------------------------------------------------------
ABS ; ACC = |psi_qr_fe| (Q15)
; ARP=AR0, AR0->FR4, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SACL *- ; FR4 = psi_qr_p = |psi_qr_fe| (Q15)
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SUB * ; ACC = |psi_qr_fe|-|psi_dr_fe| (Q15)
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
BCND LARGE_QD,GT ; Branch to LARGE_QD if |psi_qr_fe|>|psi_dr_fe|
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
BCND EQ_QD,EQ ; Branch to EQ_QD if |psi_qr_fe|=|psi_dr_fe|
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SMALL_QD ; Here, |psi_qr_fe|/|psi_dr_fe| is less than 1.
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
ADRK #1 ; ARP=AR0, AR0->FR4, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC *-,15 ; ACC = |psi_qr_fe| left shifted by 15 (psi_qdr=Q15)
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
RPT #15 ; Repeat SUBC 16 times
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SUBC * ; Dividing |psi_qr_fe|/|psi_dr_fe|
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SBRK #1 ; ARP=AR0, AR0->FR2, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SACL * ; FR2 = psi_qdr = |psi_qr_fe|/|psi_dr_fe| (Q15)
; ARP=AR0, AR0->FR2, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
; Looking at the table for arctan(|psi_qr_fe|/|psi_dr_fe|) directly
LACC *-,9 ; ACC = psi_qdr/128
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SACH * ; FR1 = ptr_fe = psi_qdr/128
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC #ATANTAB_45 ; ACC = &ATANTAB_45
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
ADD *,AR2 ; ACC = &ATANTAB_45 + ptr_fe
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe, ARP=AR2
;----------------------------------------------------------------------------------
SBRK #33 ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
TBLR * ; theta_r_fe = arctan(|psi_qr_fe|/|psi_dr_fe|) (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
B DIV_QD_END ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
LARGE_QD ; Here, |psi_qr_fe|/|psi_dr_fe| is greater than 1.
; So, |psi_dr_fe|/|psi_qr_fe| < 1 is computed instead.
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC *+,15 ; ACC = |psi_dr_fe| left shifted by 15 (psi_qdr=Q15)
; ARP=AR0, AR0->FR4, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
RPT #15 ; Repeat SUBC 16 times
; ARP=AR0, AR0->FR4, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SUBC * ; Dividing |psi_dr_fe|/|psi_qr_fe|
; ARP=AR0, AR0->FR4, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR0, AR0->FR2, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SACL * ; FR2 = psi_qdr = |psi_dr_fe|/|psi_qr_fe|
; ARP=AR0, AR0->FR2, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
; Looking at the table for arctan(|psi_dr_fe|/|psi_qr_fe|)
LACC *-,9 ; ACC = psi_qdr/128
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SACH * ; FR1 = ptr_fe = psi_qdr/128
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC #ATANTAB_45 ; ACC = &ATANTAB_45
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
ADD *,AR2 ; ACC = &ATANTAB_45 + ptr_fe
; ARP=AR0, AR0->FR1, AR2->psi_qr_fe, ARP=AR2
;----------------------------------------------------------------------------------
SBRK #33 ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
TBLR * ; theta_r_fe = arctan(|psi_dr_fe|/|psi_qr_fe|) (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
LACC #8192 ; ACC = 8192 = 90 degree (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SUB * ; ACC = 90 - theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SACL * ; theta_r_fe = 90 - theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
B DIV_QD_END ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
EQ_QD ; Here, |psi_qr_fe|/|psi_dr_fe| = 1
; i.e., |psi_qr_fe| = |psi_dr_fe|
; ARP=AR0, AR0->FR3, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR0, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
MAR *,AR2 ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SBRK #33 ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SPLK #4096,* ; theta_r_fe = 45 degree (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
DIV_QD_END ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
; Determine the quadrant by looking at signs of psi_dr_fe and psi_qr_fe
ADRK #32 ; ARP=AR2, AR0->FR1, AR2->psi_dr_fe
;----------------------------------------------------------------------------------
LACC *+ ; ACC = psi_dr_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
BCND NEG_D,LT ; Branch to NEG_D if psi_dr_fe < 0
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
POS_D ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC * ; ACC = psi_qr_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
BCND POS_D_NEG_Q,LT ; Branch to POS_D_NEG_Q if psi_qr_fe < 0
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
POS_D_POS_Q ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
B QUADRANT_QD_END ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
POS_D_NEG_Q ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC #32767 ; ACC = 32767 = 360 degree (Q15)
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SBRK #33 ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SUB * ; ACC = 360 - theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SACL * ; theta_r_fe = 360 - theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
B QUADRANT_QD_END ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
NEG_D ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC * ; ACC = psi_qr_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
BCND NEG_D_NEG_Q,LT ; Branch to NEG_D_NEG_Q if psi_qr_fe < 0
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
NEG_D_POS_Q ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC #16384 ; ACC = 16384 = 180 degree (Q15)
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SBRK #33 ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SUB * ; ACC = 180 - theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SACL * ; theta_r_fe = 180 - theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
B QUADRANT_QD_END ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
NEG_D_NEG_Q ; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
LACC #16384 ; ACC = 16384 = 180 degree (Q15)
; ARP=AR2, AR0->FR1, AR2->psi_qr_fe
;----------------------------------------------------------------------------------
SBRK #33 ; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
ADD * ; ACC = 180 + theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SACL * ; theta_r_fe = 180 + theta_r_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
QUADRANT_QD_END
;----------------------------------------------------------------------------------
_aci_fe_calc_exit:
MAR *,AR1 ; can be removed if this condition is met on
; every path to this code. (i.e., ARP=AR1 here)
SPM 0
CLRC OVM
CLRC SXM
SBRK #(__aci_fe_calc_framesize+1)
LAR AR0,*-
PSHD *
RET
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -