📄 fx_wav.asm
字号:
;===========================================================
;
; File Name :fx_wav_frm_tq.asm
;
; Originator :HUST;
; Description :This file contain source code for Fixed point flux function
; implemented using table look-up and linear interpolation technique
;
; Date : 14/07/2003 (DD/MM/YYYY)
;==========================================================
;
;
; Routine Name : Generic Function
; Routine Type : C Callable
;
; Description :
; signed int fx_tq(signed int cur,singned int theta)
;===================================================================
; Description:
; ________
; | |
; Torque Q15----->|FXTQ |----->o output
; positon Q15----->| |
; |________|
;
;===================================================================
;===================================================================
;Function Local Frame
;===================================================================
; ______
; |______| <- thcd Stack Pointer (FP+5) <---AR1
; |______| <- thcd Stack Pointer (FP+4) <---AR1
; |______| <- thcd Stack Pointer (FP+3) <---AR1
; |______| <- thcd Stack Pointer (FP+2) <---AR1
; |______| <- thabTemp (FP+1)
; |______| <- Ta Register to Register Tfr (FP) <---AR0
; |______| <- Old FP (FP-1)
; |______| <- Return Address of the caller (FP-2)
; |______| <- Module handle 1 (FP-3)
; |______| <- Module handle 1 (FP-4)
; ==================================================================
; Module definition for external referance
.ref FLUX_TAB
.def _fx_tq
__fx_tq_frs .set 00006h ; Local frame size for this routine
.text
_fx_tq:
POPD *+ ; Store the return address in s/w stack
SAR AR0, *+ ; Store caller's Frame Pointer
SAR AR1,*
LAR AR0,#__fx_tq_frs
LAR AR0,*0+,AR2 ; Create Local frame
;
;
SETC SXM
;judge the sub-tables and their start addresses
LAR AR2,#0FFFDh ;ARP=AR2, AR2=-3;
MAR *0+ ;ARP=AR2, AR2=FP-3--->Module Handle
LACL * ;ARP=AR2, AR2=FP-3, ACC=input torque(abcd)
; SUB #1000h
ADRK #6 ;ARP=AR2, AR2=FP+3
SACH *,4 ;ARP=AR2, AR2=FP+3, (FP+3)=torque_index(a)
LT * ;ARP=AR2, AR2=FP+3
MPY #257
SPL * ;ARP=AR2, AR2=FP+3
AND #0FFFh ;ARP=AR2, AR2=FP+3
ADRK #3 ;ARP=AR2, AR2=FP+6
SACL *,6 ;ARP=AR2, AR2=FP+6,(FP+6)=bcd
;calculate z1 for T1
LAR AR2,#0FFFCh ; ARP=AR2, AR2=-4
MAR *0+ ; ARP=AR2, AR2=FP-4 --->Module Handle
LACL * ; ARP=AR2, AR2=FP-4, Acc= input(abcd)
ADRK #5 ; ARP=AR2, AR2=FP+1
SFL
SACH *+,7 ; ARP=AR2, AR2=FP+2, (FP)=index(ab)
AND #01FEh ; ARP=AR2, AR2=FP+2, Acc=offset(cd) in Q15
SACL *-,6 ; ARP=AR2, AR2=FP+1, (FP+2)=offset(cd) in Q15
LACC #FLUX_TAB ; ARP=AR2, AR2=FP+1, Acc=SINTAB_360
ADDS * ; APR=AR2, AR2=FP+1
ADRK #2 ; ARP=AR2, AR2=FP+3
ADDS * ; ARP=AR2, AR2=FP+3, Acc=SINTAB_360+index --->y1
SBRK #3 ; ARP=AR2, AR2=FP
TBLR *+ ; ARP=AR2, AR2=FP+1, (FP)= y1
LT * ; ARP=AR2, AR2=FP+1, TREG=offset(cd) in Q15
ADD #1h ; ARP=AR2, AR2=FP+1, ACC=SINTAB_360+index+1 --->y2
TBLR * ; ARP=AR2, AR2=FP+1, (FP+1)=y2
LACL *- ; ARP=AR2, AR2=FP, (FP)=y1, Acc=(FP+1)=y2
SUB *+ ; ARP=AR2, AR2=FP+1, (FP+1)=y2, Acc=y2-y1
SACL * ; ARP=AR2, AR2=FP+1, (FP+1)=y2-y1
MPY * ; ARP=AR2, AR2=FP+1, PREG= (y2-y1)*offset (Q30=Q15*Q15)
PAC ; ARP=AR2, AR2=FP+1, Acc=(y2-y1)*offset (Q30)
SACH *-,1 ; ARP=AR2, AR2=FP, (FP)=y1, (FP+1)=(y2-y1)*offset (Q15)
LACC *+ ; ARP=AR2, AR2=FP+1, Acc= y1
ADD * ; ARP=AR2, Acc=y1+(y2-y1)*offset
ADRK #3 ; ARP=AR2, AR2=FP+4;
SACL *
;calculate z2 for T2
LAR AR2,#0FFFCh ; ARP=AR2, AR2=-4
MAR *0+ ; ARP=AR2, AR2=FP-4 --->Module Handle
LACL * ; ARP=AR2, AR2=FP-4, Acc= input(abcd)
ADRK #5 ; ARP=AR2, AR2=FP+1
SFL
SACH *+,7 ; ARP=AR2, AR2=FP+2, (FP)=index(ab)
AND #01FEh ; ARP=AR2, AR2=FP+2, Acc=offset(cd) in Q15
SACL *-,6 ; ARP=AR2, AR2=FP+1, (FP+2)=offset(cd) in Q15
LACC #FLUX_TAB ; ARP=AR2, AR2=FP+1, Acc=SINTAB_360
ADDS * ; ARP=AR2, AR2=FP+1
ADRK #2 ; ARP=AR2, AR2=FP+3
ADDS * ; ARP=AR2, AR2=FP+3, Acc=SINTAB_360+index --->y1
ADD #257 ; ARP=AR2, AR2=FP+3
SBRK #3 ; ARP=AR2, AR2=FP
TBLR *+ ; ARP=AR2, AR2=FP+1, (FP)= y1
LT * ; ARP=AR2, AR2=FP+1, TREG=offset(cd) in Q15
ADD #1h ; ARP=AR2, AR2=FP+1, ACC=SINTAB_360+index+1 --->y2
TBLR * ; ARP=AR2, AR2=FP+1, (FP+1)=y2
LACL *- ; ARP=AR2, AR2=FP, (FP)=y1, Acc=(FP+1)=y2
SUB *+ ; ARP=AR2, AR2=FP+1, (FP+1)=y2, Acc=y2-y1
SACL * ; ARP=AR2, AR2=FP+1, (FP+1)=y2-y1
MPY * ; ARP=AR2, AR2=FP+1, PREG= (y2-y1)*offset (Q30=Q15*Q15)
PAC ; ARP=AR2, AR2=FP+1, Acc=(y2-y1)*offset (Q30)
SACH *-,1 ; ARP=AR2, AR2=FP, (FP)=y1, (FP+1)=(y2-y1)*offset (Q15)
LACC *+ ; ARP=AR2, AR2=FP+1, Acc= y1
ADD * ; ARP=AR2, AR2=FP+1,Acc=y1+(y2-y1)*offset
ADRK #4 ; ARP=AR2, AR2=FP+5;
SACL *+ ; ARP=AR2, AR2=FP+6
;calculate z=(z2-z1)*offset
LT *- ; ARP=AR2, AR2=FP+5,
LACL *- ; ARP=AR2, AR2=FP+4, (FP+4)=z1, Acc=(FP+5)=z2
SUB *+ ; ARP=AR2, AR2=FP+5, (FP+5)=z2, Acc=z2-z1
SACL * ; ARP=AR2, AR2=FP+5, (FP+5)=z2-z1
MPY * ; ARP=AR2, AR2=FP+5, PREG= (z2-z1)*offset (Q30=Q15*Q15)
PAC ; ARP=AR2, AR2=FP+5, Acc=(z2-z1)*offset (Q30)
SACH *-,1 ; ARP=AR2, AR2=FP+4, (FP+4)=z1, (FP+5)=(z2-z1)*offset (Q15)
LACC *+ ; ARP=AR2, AR2=FP+5, Acc= z1
ADD *,AR1 ; ARP=AR2, AR2=FP+5,Acc=z1+(z2-z1)*offset
SBRK #(__fx_tq_frs+1) ;Clear the local frame
LAR AR0,*- ; Retrive Caller's frame pointer
PSHD * ; push the return address to TOS
RET ; Return to the caller
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -