📄 q1atan.asm
字号:
;===================================================================================
;
; File Name :q1atan.asm
;
; Originator :Digital Control Systems Group
; Texas Instruments
;
; Description :This file contain source code for atan
;
; |-----------|
; | |
; atan_input --->| QATAN |-------> atan_output
; | |
; |-----------|
;
; Date : 26/12/2000
;==========================================================
; Routine Name : Generic Function
; Routine Type : ASM only;
;
; Algorithm :
; atan(x): where 'x' is in 16.16 format
; = 0.318253*x + 0.003314*x^2 - 0.130908*x^3 + 0.068542*x^4 - 0.009159*x^5; if x<1
; = 0.5-atan(1/x); if x>=1
;
;
;====================================================================================
; Pseudo Code
;==================================================================================
;
; signed short int qatan(signed long int x)
; {
; signed short int y, at;
; signed long int absx;
;
; absx=abs(x) /* Find tha Absolute value of 'x' */
; if(absx<65536) /* True, If the x<1, (Q16 value of 1 is 65536) */
; y=lsw(absx); /* Take only the 16 LSbits of X(32 bit number) */
; else /* False, If X>=1 */
; y=idiv(1,absx); /* Integer divide: Find the 1/x of 32 bit number(16.16 format) */
; /* Only 16 bits of the quotient are obtained */
;
; at=appx(y); /* Use fixed point 16 bit approximation to find atan */
; /* Result of the approximation 'at' will be in Q15 format */
; if(absx>=65536) /* True, If the X>=1, (Q16 value of 1 is 65536) */
; at=0x4000-at; /* 0x4000 corresponds to 0.5 in Q15 format */
;
; if(x>0)
; return at; /* return atan(absx) for positive 'x' */
; else
; return -at; /* return -atan(absx) for negative 'x' */
;
; }
;
;==============================================================================
;======================================================================
; Module Usage: Copy this section to main system file
; .ref qatan
; .ref atan_input,atan_output
;========================================================================
; Constant required for obtaining Tylor Series Approximation of ATAN
__a1 .set 05179h ; 0.318253 scaled by 2^16
__a2 .set 06c98h ; 0.003314 scaled by 2^23
__a3 .set 0bcfah ; -0.130908 scaled by 2^17
__a4 .set 04630h ; 0.068542 scaled by 2^18
__a5 .set 0169fh ; -0.009159 scaled by 2^18 for Immediate MPY
__b0 .set 04000h ; 0.5 scaled by 2^15
; Module definition for external referance
.def _qatan
; Input/output Symbol Delaration for external referance
.def atan_input, atan_output
; I/O and Local symbol definition
atan_input .usect "qmath",2,1
atan_output .usect "qmath",1
temp .usect "qmath",2
; ATAN module definition
_qatan:
SETC SXM
SETC OVM
SPM #0
LAR AR3,#(atan_input+1) ; AR3->MSW of atan_input
LAR AR0,#temp ; AR0->temp[0]
LAR AR2,#(temp+1) ; AR2->temp[1]
MAR *,AR3 ; ARP=AR3
;=======================================================================================================
; From Here -- The code is same as CcA
;=======================================================================================================
LACC *-,16 ; Load the MSW of x in ACCH
ADDS *+,AR4 ; Add the LSW to ACCL as unsigned number
; ACC=X(32 bit number)
BCND positive,GEQ ; If 'X' >=0, then go to positive
ABS ; If 'X' is negative, convert it to positive
positive:
LAR AR4,#0fff1h ; AR4=-15
RPT #14 ; AR4=E-15, Max(E)=15 as NORM is executed 15 times
NORM *+ ; * Remove upto 15 extra sign bits to obtain
NOP ; 16 significat bits from 32 bit |x| for 16 bit div.
NOP ; * Keep track of the no of extra sign bits removed in AR4
MAR * ,AR2 ;
SACH *,0,AR0 ; temp[1]= 16 significad bits of 32 bit |x|
; (16-E.E format)
SAR AR4,* ; temp[0]=E-15
BCND return,EQ ; If input is ZERO, Return with ZERO
; temp[0]=0, If the input is Zero
LACC * ; ACC=E-15
BCND noinv,EQ ; If |X| were <1, then use directly the series apprx
; Otherwise obtain 1/|X| before using the series apprx
; 1/|X| is obtained by 16 bit division, though the X input was in 16.16 format,
; The strategy adopted here uses the 16 significant bits obtained through removing
; upto 15 sign bits for division.
ADD #010h ; ACC=E-15+16=E+1
SACL * ; temp[0]=E+1
LT * ; TREG=E+1
SPLK #4000h,* ; temp[0]=#1 in 2.14 format
LACT *,AR2 ; #1 in 2.14 scaled by 2^(E+1)
; =#1 in 17-E.15+E in ACC
RPT #15 ; Perform the division= 1/temp
SUBC * ; (17-E.15+E)/(16-E.E)= 1.15 format
SACL * ; Store |x| in 1.15 format
BIT *,0
BCND noinv,NTC ; If the MSB of the Quotient '1' then saturate to 7fff
SPLK #07fffh,* ; Change 0x8000 to max Q15 value of 0x7fff
noinv: MAR *,AR2
LT * ; TREG=x in Q15
MPY #__a5 ; P=x*a5 in Q33
LACC #__a4,15 ; ACC=a4 in Q33
APAC ; ACC=a4+x*a5 in Q33
SACH * ; Store a4+x*a5 in Q17
MPY * ; P=x*(a4+x*a5) in Q32
LACC #__a3,15 ; ACC=a3 in Q32
APAC ; ACC=a3+x*(a4+x*a5) in Q32
SACH *,1 ; Store a3+x*(a4+x*a5) in Q17
MPY * ; P=x*(a3+(a4+x*a5)) in Q32
LACC #__a2,9 ; ACC=a2 in Q32 format
APAC ; ACC=a2+x*(a3+(a4+x*a5)) in Q32
SACH * ; Store a2+x*(a3+(a4+x*a5)) in Q16
MPY * ; P=x*(a2+x*(a3+(a4+x*a5))) in Q31
LACC #__a1,15 ; ACC=a1 in Q31 format
APAC ; ACC=a1+x*(a2+x*(a3+(a4+x*a5))) in Q31
SACH *,1 ; Store a1+x*(a2+x*(a3+(a4+x*a5))) in Q16
MPY * ; P=x*(a1+x*(a2+x*(a3+(a4+x*a5)))) in Q31
PAC ; ACC=x*(a1+x*(a2+x*(a3+(a4+x*a5)))) in Q31
SACH *,0,AR0 ; Store x*(a1+x*(a2+x*(a3+(a4+x*a5)))) in Q15
; temp[1]=Arctan(x)
SAR AR4,* ; temp[0]=E-15
LACC *,0,AR2 ; ACC=E-15
BCND nosub,EQ ; If |X|<1, then skip the offset adjustment
LACC * ; ACC=Arctan(x) in Q15
NEG
ADD #__b0 ; ACC= 0.5-Arctan(1/x)
SACL *
nosub: LACC *,AR3
;==============================================================================
; Upto this point - the code is same as CcA
;=============================================================================
return: BIT *,0,AR2
BCND noneg,NTC ; if X is positive, then skip the negation
NEG
noneg: CLRC OVM
LAR AR2,#atan_output
SACL *
RET
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -