arct2.asm

CCS3.3自带的TI 5400系列DSP的dsplib文件。文档说明可以在TI公司网站上下载。

;***********************************************************
; Version 2.20.01                                           
;***********************************************************
;****************************************************************
;  Function:	atan2_16
;  Description: arctangent 2 implementation
;
;  Copyright Texas instruments Inc, 1998
;----------------------------------------------------------------
;  Revision History:
;  1.00, A. Aboagye, 8/31/98 - Original release. Started from code by Jeff Axelrod
;  1.10, A. Aboagye, 10/7/99 - Fixed frame error in Quadrant 1
;********************************************************************************
	.mmregs

	.def	_atan2_16

	.if __far_mode
OFFSET	.set 1
	.else
OFFSET	.set 0
	.endif


; Command-line stack arguments
ARG_Y .set 6 + OFFSET
ARG_Z .set 7 + OFFSET
ARG_N .set 8 + OFFSET

; ARXY_ means Xmem/Ymem register
AR_X .set ar0				; Points to SP_INDEX
AR_Y .set ar1 
AR_Z .set ar2
AR_N .set ar3 
AR_ZEXP .set ar4
AR_TABLE .set ar5

; Stack local variables
SP_INVYETABLE	.set 0
SP_XNORM	.set 1
SP_TEMP		.set 2
SP_NUMERATOR	.set 3
SP_QUADRANT	.set 4
SP_ONE		.set 5
FSZ		.set 6			; # of local variables


_atan2_16:

	pshm	ar1
	pshm	ar6
	pshm	ar7

        PSHM    ST0                                 ; 1 cycle
        PSHM    ST1                                 ; 1 cycle
        RSBX    OVA                                 ; 1 cycle
        RSBX    OVB                                 ; 1 cycle

; Process command-line arguments

	stl	a, *(AR_X)
	mvdk	*sp(ARG_Y), AR_Y
	mvdk	*sp(ARG_Z), AR_Z
	st	#atancoeffs, *(AR_TABLE)


; N=N-1
	ld	*sp(ARG_N), a
	sub	#1, a
	stlm	a, AR_N    


; Can't access local variables before this instruction
; Can't access command-line arguments after this instruction

	frame	#-FSZ          
	nop
	st	#32767, *sp(SP_ONE)


; Initialize constants

	ssbx SXM			; MUST set sign-extension mode.
	ssbx OVM			; MUST turn overflow mode on.


LOOP:    
	ld	*AR_X, a
	and	#08000h, 2, a
	ld	*AR_Y, b
	and	#08000h, 1, b
	add	b, a			; a=2*abs(x<0)+abs(y<0)
	sth	a, *SP(SP_TEMP)
	mpy	*SP(SP_TEMP), #7, a
	add	#QUADRANT1, a
	stl	a, *sp(SP_QUADRANT)
	ld	*AR_X+, 16, a		; Since we're dividing y/x, we want x>y.
	abs	a
	sth	a, *sp(SP_NUMERATOR)	; Numerator defaults to X
	ld	*AR_Y+, 16, b
	abs	b
	max	a
	bc	XGTY, nc

XLTY:  
	; a=y
	call	atan2			; compute atan(x/y).  Result in AH
	add	#-16384, 16, a		; AH = -0.5+atan(x/y)
	neg	a			; AH = 0.5-atan(x/y)
	ld	*SP(SP_QUADRANT), b
	bacc	b

XGTY:
	; a=x
	sth	b, *sp(SP_NUMERATOR)	; Numerator=y
	call	atan2			; compute atan(y/x).  Result in AH
	ld	*SP(SP_QUADRANT), b
	bacc	b

QUADRANT1:
	sth	a, *AR_Z+		; Z[i]=a
	banz	LOOP, *AR_N-
;	frame	#FSZ
        nop
	b	DONE
	nop

QUADRANT4:
	neg	a
	sth	a, *AR_Z+		; Z[i]=-a
	banz	LOOP, *AR_N-
	b	DONE
	nop

QUADRANT2:	
	ld	*sp(SP_ONE), 16, b
	sub	a, b
	sth	b, *AR_Z+		; Z[i]=32767-a
	banz	LOOP, *AR_N-
	b	DONE

QUADRANT3:      
	sub	*sp(SP_ONE), 16, a
	sth	a, *AR_Z+		; Z[i]=a-32767
	banz	LOOP, *AR_N-

DONE:	
	frame	#FSZ
	ld	#0, A
	xc	1, AOV
	ld	#1, A

        POPM    ST1                                 ; 1 cycle
        POPM    ST0                                 ; 1 cycle

	popm	ar7
	popm	ar6
	popm	ar1

	.if	__far_mode
	fret
	.else
	ret
	.endif


*****************************************************************************

atan2:

; BEGIN Compute Z=(1/a)*(*AR_Y)
	ssbx	OVM 			; saturate


; Normalize the input value X.

	exp	a
	nop
	nop
	norm	a
	
	ld	#0, b
	st	t, *sp(SP_TEMP+1)
	add	*sp(SP_TEMP+1), b
	stl	b, *sp(SP_TEMP+1)
	
	sth	a, *sp(SP_XNORM+1)	; AR2 points to appropriate Ye value in table.
	sfta	a, -1			; Estimate the first Ym value.
	xor	#01FFFh, 16, a		
	sth	a, *AR_Z


; First two iterations:

	.loop	2
	ld	*AR_Z, 15, a		; Calculate Ym = 2*Ym - Ym^2*X
	ld	*AR_Z, t
	mpy	*sp(SP_XNORM+1), b
	sth	b, 1, *AR_Z
	mpy	*AR_Z, b
	sub	b, 1, a
	sth	a, 2, *AR_Z
	.endloop


; Final iteration: - this code is same as above loop, except 
; last instruction omitted

	ld	*AR_Z, 15, a		; Calculate Ym = 2*Ym - Ym^2*X
	ld	*AR_Z, t
	mpy	*sp(SP_XNORM+1), b
	sth	b, 1, *AR_Z
	mpy	*AR_Z, b
	sub	b, 1, a

	st	#07000h, *AR_Z		; Make sure that 8000h <= Ym < 7FFFh
	add	*AR_Z, 16, a
	sub	*AR_Z, 16, a
	sub	*AR_Z, 16, a
	add	*AR_Z, 16, a
	mpya	*sp(SP_NUMERATOR+1)
	sfta	b, 5
	ld	*sp(SP_TEMP+1), ASM
	sth	b, ASM,*AR_Z


; END Compute Z=(1/a)*(*AR_Y)  

; BEGIN Compute Z=atan(Z)>>2

	ssbx	FRCT			; MUST Set Product shift mode to +1
	ld	*AR_Z, t     
	ld	*AR_TABLE+, 16, a
	ld	*AR_TABLE+, 16, b
	poly	*AR_TABLE+
	poly	*AR_TABLE+
	poly	*AR_TABLE+
	poly	*AR_TABLE
	poly	*AR_TABLE
	st	#atancoeffs, *(AR_TABLE)
	sfta	a, -1

	sth	a, *AR_Z
	retd
	rsbx	FRCT
	rsbx	OVM


********************************************************************************

	.data    

atancoeffs:
	.word 0xfda8 ; -0.009159*2*32767.999*x^5
	.word 0x118b ; +0.068542*2*32767.999*x^4
	.word 0xde7c ; -0.130908*2*32767.999*x^3    
	.word 0x00d9 ; +0.003314*2*32767.999*x^2
	.word 0x5179 ; +0.318253*2*32767.999*x^1
	.word 0x0000 ; 0*x^0

;end of file. please do not remove. it is left here to ensure that no lines of code are removed by any editor