cordic.asm

Cordic algorithm description and C/Asm51 sourcecode examples

;-----------------------------------------------------------;
; Given an angle -90.0 to +90.0 this routine calculates     ;
; a the 16-bit sine and cosine using CORDIC rotations.  All ;
; the math in this routine uses signed 16.16 fixed point    ;
; notation to simply calculations.  The high byte is        ;
; wasted in most calculations, including the table for      ;
; atan(1/2^i), but I think it is a good tradeoff.           ;
;                                                           ; 
;                                                           ;
; INPUT:       R0    Points to 32-bit Angle in RAM          ;
;                    R0->MSB,,,(R0+3)->LSB.                 ;
;              R0+4  Points to 32-bit position              ;
;                    in RAM to return the COS               ;  
;              R0+8  Points to 32-bit position              ;
;                    in RAM to return the SIN               ;
;                                                           ;
; OUTPUT:      R0    Points to 32-bit in RAM,               ;
;                    R0->MSB,,,(R0+3)->LSB.                 ;
;              R0+4  Points to 32-bit position              ;
;                    in RAM to return the COS               ;
;              R0+8  Points to 32-bit position              ;
;                    in RAM to return the SIN               ;
;                                                           ;
; DESTROYS:    None.                                        ;
;                                                           ;
; Register Usage:                                           ;
;     R2       Points to Angle                              ;
;     R2+4     Points to X                                  ;
;     R2+8     Points to Y                                  ;
;     R3       Points to DA                                 ;
;     R3+4     Points to DX                                 ;
;     R3+8     Points to DY                                 ;
;     R3+12    Points to Z                                  ;
;     R6       Loop counter                                 ;
;-----------------------------------------------------------;
cordic:
   push  0x000
   push  0x001                ; Save registers
   push  0x002
   push  0x003
   push  0x006
   push  psw
   push  dpl
   push  dph
   push  acc
   push  b

   mov   a,r0
   mov   r2,a                 ; Save copy of R0 pointer in R2

; Allocate space on stack for DA, DX, DY
; and save pointer in R1
   mov   a,sp                 ; Get copy of stack pointer
   mov   r3,a                 ; Save pointer in R1
   inc   r3                   ; Point to next free location
   add   a,#16                ; Make room for 4 variables
   mov   sp,a                 ; Adjust stack

; Initialize X to K           
   mov   a,r2                 
   add   a,#4                 ; Get pointer to X
   mov   r0,a
   mov   dptr,#cordick        ; Get pointer to K
   lcall copy32p              ; K->X

; Initial Y=0
   mov   a,r2
   add   a,#8                 ; Get pointer to Y
   mov   r0,a
   
   mov   @r0,#0
   inc   r0
   mov   @r0,#0
   inc   r0
   mov   @r0,#0
   inc   r0
   mov   @r0,#0

; Initialize Z to Angle
   mov   a,r2
   mov   r0,a                 ; Get pointer to angle in R0   
   mov   a,r3                 ;
   add   a,#12                ; Get pointer to Z
   mov   r1,a                 ; Save in R1
   lcall copy32               ; Initialize Z

; Main Loop

   lcall send_crlf
   mov   r6,#0                ; Initialize counter
cordic1:      

; DX:=X/2^i
   lcall cordicdx

; Get DY:=Y/2^i
   lcall cordicdy
   
; Get DA:=ATAN(1/2^i)
   mov   a,r6                 ; Get current count
   mov   b,#4                 ; Four bytes per value
   mul   ab                   ; Get position in ACC
   mov   dptr,#cordic_tbl     ; Get pointer to atan(1/2^i) lookup table
   lcall util_adcbad          ; DPTR:=DPTR+AB
   mov   a,r3
   mov   r0,a                 ; Get pointer to DA in R0
   lcall copy32p              ; DA:=ATAN(1/2^i)

; Check sign of Z

   mov   a,r3
   add   a,#12                ; Get pointer to Z
   mov   r0,a
   mov   a,@r0                ; Fetch MSB of Z
   jb    acc.7,cordic2       ; Jump if Z is negative
   
; X:=X-DY
   mov   a,r2
   add   a,#4                 ; Get pointer to X in R0
   mov   r0,a
   mov   a,r3                 
   add   a,#8                 ; Get pointer to DY in R1
   mov   r1,a
   lcall sub32s               ; X:=X-DY
   
; Y:=Y+DX
   mov   a,r2
   add   a,#8                 ; Get pointer to Y in R0
   mov   r0,a
   mov   a,r3
   add   a,#4                 ; Get pointer to DX in R1
   mov   r1,a
   lcall add32                ; Y:=Y+DX

; Z:=Z-DA
   mov   a,r3
   add   a,#12                ; Get pointer to Z
   mov   r0,a
   mov   a,r3
   mov   r1,a                 ; Get pointer to DA
   lcall sub32s               ; Z:=Z-DA
   
   sjmp  cordic3             ; Loop again
cordic2:

; X:=X+DY
   mov   a,r2
   add   a,#4                 ; Get pointer to X in R0
   mov   r0,a
   mov   a,r3                 
   add   a,#8                 ; Get pointer to DY in R1
   mov   r1,a
   lcall add32                ; X:=X+DY

; Y:=Y-DX
   mov   a,r2
   add   a,#8                 ; Get pointer to Y in R0
   mov   r0,a
   mov   a,r3
   add   a,#4                 ; Get pointer to DX in R1
   mov   r1,a
   lcall sub32s               ; Y:=Y-DX

; Z:=Z+DA
   mov   a,r3
   add   a,#12                ; Get pointer to Z
   mov   r0,a
   mov   a,r3
   mov   r1,a                 ; Get pointer to DA
   lcall add32                ; Z:=Z+DA
   
cordic3:

   inc   r6                   ; Increment count
   cjne  r6,#16,cordic20
   sjmp  cordic_exit

cordic20:
   ljmp  cordic1

cordic_exit:

; Deallocate stack space for DA, DX, DY
   mov   a,sp                 ; Get stack pointer
   clr   c                    ; Clear for subb
   subb  a,#16                ; Adjust stack
   mov   sp,a                 ; Save back

   pop   b
   pop   acc
   pop   dph
   pop   dpl
   pop   psw
   pop   0x006
   pop   0x003
   pop   0x002                ; Restore registers
   pop   0x001
   pop   0x000
   ret
   
; This subroutine calculates DX:=X/2^i
cordicdx:
   mov   a,r2
   add   a,#4                 ; Get pointer to X in R0
   mov   r0,a
   mov   a,r3
   add   a,#4                 ; Get pointer to DX in R1
   mov   r1,a                 
   lcall copy32               ; DX:=X
   mov   a,r1                 
   mov   r0,a                 ; Get pointer to DX in R0
   mov   a,r6                 ; Get count in ACC
   cpl   a                    ; Get 2's complement
   inc   a   
   lcall shift32s             ; DX:=X/2^i           
   ret


; This subroutine calculates DY:=Y/2^i
cordicdy:
   mov   a,r2
   add   a,#8                 ; Get pointer to Y in R0
   mov   r0,a
   mov   a,r3
   add   a,#8                 ; Get pointer to DY in R1
   mov   r1,a                 
   lcall copy32               ; DY:=Y
   mov   a,r1                 
   mov   r0,a                 ; Get pointer to DY in R0
   mov   a,r6                 ; Get count in ACC
   cpl   a                    ; Get 2's complement
   inc   a   
   lcall shift32s             ; DY:=Y/2^i         
   ret