📄 smopos.asm
字号:
; Looking at the table for arctan(|esalfa|/|esbeta|) directly
LACC *-,9 ; ACC = es_qd/128
; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
SACH * ; FR0 = ptr_smo = es_qdr/128
; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LACC #ATANTAB_45 ; ACC = &ATANTAB_45
; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
ADD *,AR2 ; ACC = &ATANTAB_45 + ptr_smo
; ARP=AR0, AR0->FR0, AR2->esalfa, ARP=AR2
;----------------------------------------------------------------------------------
ADRK #17 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
TBLR * ; thetau = arctan(|esalfa|/|esbeta|) (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
B SMO_DIV_QD_END ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SMO_LARGE_QD ; Here, |esalfa|/|esbeta| is greater than 1.
; So, |esbeta|/|esalfa| < 1 is computed instead.
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
LACC *+,15 ; ACC = FR2 = |esbeta| left shifted by 15 (es_qd=Q15)
; ARP=AR0, AR0->FR3, AR2->esalfa
;----------------------------------------------------------------------------------
RPT #15 ; Repeat SUBC 16 times
; ARP=AR0, AR0->FR3, AR2->esalfa
;----------------------------------------------------------------------------------
SUBC * ; Dividing |esbeta|/|esalfa|
; ARP=AR0, AR0->FR3, AR2->esalfa
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR0, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
SACL * ; FR1 = es_qd = |esbeta|/|esalfa|
; ARP=AR0, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
; Looking at the table for arctan(|esbeta|/|esalfa|)
LACC *-,9 ; ACC = es_qd/128
; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
SACH * ; FR0 = ptr_smo = es_qd/128
; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LACC #ATANTAB_45 ; ACC = &ATANTAB_45
; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
ADD *,AR2 ; ACC = &ATANTAB_45 + ptr_smo
; ARP=AR0, AR0->FR0, AR2->esalfa, ARP=AR2
;----------------------------------------------------------------------------------
ADRK #17 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
TBLR * ; thetau = arctan(|esbeta|/|esalfa|) (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
LACC #8192 ; ACC = 8192 = 90 degree (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SUB * ; ACC = 90 - thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SACL * ; thetau = 90 - thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
B SMO_DIV_QD_END ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SMO_EQ_QD ; Here, |esalfa|/|esbeta| = 1
; i.e., |esalfa| = |esbeta|
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
MAR *,AR2 ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
ADRK #17 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SPLK #4096,* ; thetau = 45 degree (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SMO_DIV_QD_END ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
; Determine the quadrant by looking at signs of esbeta and -esalfa
SBRK #11 ; ARP=AR2, AR0->FR0, AR2->esbeta
;----------------------------------------------------------------------------------
LACC * ; ACC = esbeta (Q15)
; ARP=AR2, AR0->FR0, AR2->esbeta
;----------------------------------------------------------------------------------
SBRK #6 ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
BCND SMO_NEG_D,LT ; Branch to NEG_D if esbeta < 0
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
SMO_POS_D ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LACC * ; ACC = esalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
NEG ; ACC = -esalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
BCND SMO_POS_D_NEG_Q,LT ; Branch to POS_D_NEG_Q if -esalfa < 0
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
SMO_POS_D_POS_Q ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
ADRK #17 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
B SMO_QUADRANT_QD_END ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SMO_POS_D_NEG_Q ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LACC #32767 ; ACC = 32767 = 360 degree (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
ADRK #17 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SUB * ; ACC = 360 - thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SACL * ; thetau = 360 - thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
B SMO_QUADRANT_QD_END ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SMO_NEG_D ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LACC * ; ACC = esalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
NEG ; ACC = -esalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
BCND SMO_NEG_D_NEG_Q,LT ; Branch to NEG_D_NEG_Q if -esalfa < 0
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
SMO_NEG_D_POS_Q ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LACC #16384 ; ACC = 16384 = 180 degree (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
ADRK #17 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SUB * ; ACC = 180 - thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SACL * ; thetau = 180 - thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
B SMO_QUADRANT_QD_END ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SMO_NEG_D_NEG_Q ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LACC #16384 ; ACC = 16384 = 180 degree (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
ADRK #17 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
ADD * ; ACC = 180 + thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SACL * ; thetau = 180 + thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
SMO_QUADRANT_QD_END ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
LACC *+ ; ACC = thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetae
;----------------------------------------------------------------------------------
SACL *+ ; thetae = thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->comp_ang_flg
;----------------------------------------------------------------------------------
; End: Rotor angle calculator
;----------------------------------------------------------------------------------
; (5) Rotor angle compensator
; thetae =+ delta_table(speed reference)
; 2^cmptablel_ is delta lookup table length
; cmptable_ is delta lookup table start address
;----------------------------------------------------------------------------------
LACC *+ ; ACC = comp_ang_flg (Q0)
; ARP=AR2, AR0->FR0, AR2->speedref
;----------------------------------------------------------------------------------
BCND SMO_RT,EQ ; Branch to SMO_RT if comp_ang_flg = 0
; ARP=AR2, AR0->FR0, AR2->speedref
;----------------------------------------------------------------------------------
ANGLECOMP ; ARP=AR2, AR0->FR0, AR2->speedref
;----------------------------------------------------------------------------------
LACC *,16-cmptablel_,AR0 ; ACC = speedref
; ARP=AR2, AR0->FR0, AR2->speedref, ARP=AR0
;----------------------------------------------------------------------------------
SACH * ; FR0 = smoptemp = speedref
; ARP=AR0, AR0->FR0, AR2->speedref
;----------------------------------------------------------------------------------
LACC * ; ACC = smoptemp
; ARP=AR0, AR0->FR0, AR2->speedref
;----------------------------------------------------------------------------------
ADD #cmptable_ ; ACC = smoptemp + &cmptable_
; ARP=AR0, AR0->FR0, AR2->speedref
;----------------------------------------------------------------------------------
TBLR *,AR2 ; FR0 = delta (Q15)
; ARP=AR0, AR0->FR0, AR2->speedref, ARP=AR2
;----------------------------------------------------------------------------------
SBRK #3 ; ARP=AR2, AR0->FR0, AR2->thetau
;----------------------------------------------------------------------------------
LACC *+,AR0 ; ACC = thetau (Q15)
; ARP=AR2, AR0->FR0, AR2->thetae, ARP=AR0
;----------------------------------------------------------------------------------
ADD *,AR2 ; ACC = thetau + delta (Q15)
; ARP=AR0, AR0->FR0, AR2->thetae, ARP=AR2
;----------------------------------------------------------------------------------
AND #7FFFh ; Modulo 2*pi (or 7FFFh)
; ARP=AR2, AR0->FR0, AR2->thetae
;----------------------------------------------------------------------------------
SACL * ; thetae = thetau + delta (Q15)
; ARP=AR2, AR0->FR0, AR2->thetae
;----------------------------------------------------------------------------------
SMO_RT ; ARP=AR2, AR0->FR0, AR2->thetae
;----------------------------------------------------------------------------------
; End: Rotor angle compensator
;----------------------------------------------------------------------------------
_smopos_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 #(__smopos_calc_framesize+1)
LAR AR0,*-
PSHD *
RET
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -