ATAN2 in 6502
Entry posted by djmips
1,626 views
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.
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
Like us on Facebook
Follow us on Twitter
AtariAge on Instagram
Watch us on YouTube
Chat on Discord
4 Comments
Recommended Comments