Motorola DSP96002 - Manual (Page 643)

B-124 DSP96002 USER’S MANUAL MOTOROLA
org x:0
scale set 1.0
tantab
tanarg set 45.0*3.14159/180.0
dup tabsize
scale set scale*@cos(tanarg)
dc @tan(tanarg)
tanarg set tanarg/2.0
endm
org p:$100
;
; Do argument reduction, input in d6 in degrees
;
move #-180.0,d7.s ;get range min
fadd.x d7,d6 #1.0/360.0,d5.s ;adjust to min, get range
fmpy.x d5,d6,d6 ;reduce range
floor d6,d5 ;get int part
fsub.x d5,d6 #360.0,d5.s ;get frac part, spread
fmpy.x d5,d6,d6 ;spread fraction part to range
fadd.x d7,d6 ;adjust to min
;
; Input angle in d6 in degrees, -180 < d6 < 180
;
fabs.x d6 #90.0,d7.s ;make positive, get pi/2
move d6.s,d3.s ;save new sign
fcmp d7,d6 #180.0,d7.s ;see if greater than 90
fsub.x d6,d7 ffge ;reduce to less than 90
ftfr.x d6,d7 fflt ;transfer if no change
fneg.x d3 ffge ;flip if other quadrant
;
; First quadrant CORDIC trig computation
; Input angle in d7 in degrees
; Output d1=sine, d0=cosine
;
move #tantab,r0 ;point to tangent table
fclr d1 #scale,d0.s ;y=0, x=scale
fclr d5 #45.0,d6.s ;z=0,alp=45
do #tabsize,_cordic
fcmp d5,d7 x:(r0)+,d4.s ;angle < z? get tangent
fneg.x d4 fflt ;yes, rotate cw
fsub.x d6,d5 fflt ;yes, subtract angle
fadd.x d6,d5 ffge ;no, add angle for ccw
fmpy.x d1,d4,d2 ;y*tan
fmpy d0,d4,d2 fsub.x d2,d0 ;x*tan, x’=x-y*tan