MIPS HEAVY INDUSTRIES - ATAN2 in 6502

[RSS Bot]

Some code rolling off the heavy industries math line today.

 

I've implemented a straighforward version of a CORDIC ATAN2 routine.

I've taken a few shortcuts. The #'s are normalized by shifting left 4 (x16) and I've limited the iterations to 8.

 

The accuracy is around 1 binary radian. (Binary Radians or BRADS : 0 - 255)

 

The slowest part is the two subroutine calls to ASRN in the inner loop. The slowness is a reflection of the fact that the 6502 doesn't have a barrel shifter, and doesn't have an arithmetic shift right instruction ( and well, chokes on 16 bits a tad)

 

I still enjoyed creating this routine but it's not the fastest. I've timed it and it takes about 2100 cycles or around 27 scanlines. It could be optimized. The main speedup would be to use shifting tables I think.

 

I've got another ATAN2 routine planned but it only spits out I think 4 times less accuracy but should be a lot faster.

 

This routine could also be sped up at the expense of accuracy by using fewer iterations. However, I am going to try out this other idea before I get back to this CORDIC version.

 

 

CODE processor 6502

 

seg.u zpage

ORG $0080

 

 

theta ds 2

x ds 2

y ds 2

rt ds 2

i ds 1

ox ds 1

oy ds 1

 

HALF equ 32768

MAXL equ 8

 

seg rom

org $f000

 

Start

 

lda #<$190

sta x

lda #>$190

sta x+1

 

lda #<0

sta y

lda #>0

sta y+1

 

jsr atan2

 

jmp Start

 

 

atan2

lda x+1

sta ox

lda y+1

sta oy

 

;x * 16

asl x

rol x+1

asl x

rol x+1

asl x

rol x+1

asl x

rol x+1

 

;y * 16

asl y

rol y+1

asl y

rol y+1

asl y

rol y+1

asl y

rol y+1

 

lda #0

sta theta

sta theta+1

 

lda y+1

bpl .ypositive

 

; y = -y

lda #0

sec

sbc y

sta y

lda #0

sbc y+1

sta y+1

 

; x = -x

lda #0

sec

sbc x

sta x

lda #0

sbc x+1

sta x+1

 

.ypositive

 

lda x+1

bpl .xpositive

 

; ytemp = y

ldx y

ldy y+1

 

; y = -x

lda #0

sec

sbc x

sta y

lda #0

sbc x+1

sta y+1

 

; x = ytemp

stx y

sty y+1

 

.xpositive

 

;------------------

;

 

ldx #0

 

.cordicLoop

stx i

 

lda y+1

bpl .rotateNegative

 

;rotate positive

 

lda x

sta rt

lda x+1

sta rt+1

ldx i

beq .noshiftxp

jsr asrn

.noshiftxp

 

ldx y

ldy y+1

 

txa

clc

adc rt

sta y

tya

adc rt+1

sta y+1

 

stx rt

sty rt+1

 

ldx i

beq .noshiftyp

jsr asrn

.noshiftyp

 

lda x

sec

sbc rt

sta x

lda x+1

sbc rt+1

sta x+1

 

ldx i

lda theta

sec

sbc atantabL,x

sta theta

lda theta+1

sbc atantabH,x

sta theta+1

 

inx

cpx #MAXL

bcc .cordicLoop

bcs .done

 

.rotateNegative

 

lda x

sta rt

lda x+1

sta rt+1

ldx i

beq .noshiftxn

jsr asrn

.noshiftxn

ldx y

ldy y+1

 

txa

sec

sbc rt

sta y

tya

sbc rt+1

sta y+1

 

stx rt

sty rt+1

ldx i

beq .noshiftyn

jsr asrn

.noshiftyn

lda x

clc

adc rt

sta x

lda x+1

adc rt+1

sta x+1

 

ldx i

lda theta

clc

adc atantabL,x

sta theta

lda theta+1

adc atantabH,x

sta theta+1

 

inx

cpx #MAXL

bcs .done

jmp .cordicLoop

 

.done

; if original y was negative

lda oy

bpl .oyplus

lda theta+1

clc

adc #$80

sta theta+1

.oyplus

 

; if original x was negative

lda ox

bpl .oxplus

lda theta+1

clc

adc #$40

sta theta+1

.oxplus

 

rts

 

; arithmetic shift right xtl and xth by X

asrn

lda rt+1

.shiftr

cmp #$80

ror

ror rt

dex

bne .shiftr

sta rt+1

rts

 

 

; arithmetic shift left xtl and xth by X

asln

lda rt+1

.shiftl

asl rt

rol

dex

bne .shiftl

sta rt+1

rts

 

 

atantabL

.byte <8192, <4836, <2555, <1297, <651, <326, <163, <81, <41, <20, <10, <5, <3, <1, <1

atantabH

.byte >8192, >4836, >2555, >1297, >651, >326, >163, >81, >41, >20, >10, >5, >3, >1, >1

 

 

ORG $FFF8

.word 0 ; for supercharger

.word 0 ; nmi vector

.word Start ; start vector

.word Start ; brk vector

 

http://www.atariage.com/forums/index.php?a...;showentry=3385