Cordic trig assembly code for PIC18f

[Darrel Taylor]

There are 15 elements in the array (0-14).

Might be overwriting another variable.
<br>

[ScaleRobotics]

Once again you saved the day!

IT WORKS! I will clean it up a little bit and repost, so others may enjoy it. Looks to be about the same speed as the original author posted. Around 9,000 times per second on a 48mhz chip.

[Darrel Taylor]

IT WORKS!

SWEET!

You make it easy Walter.

I know you just got it working, so you probably don't know this yet ...
I'm curious about the accuracy.

If I remember correctly, the last version could calculate a bearing within ~200 miles.

What's the new resolution?
<br>

[ScaleRobotics]

Resolution is pretty good!

180 degrees is covered by 0 to 32,767.

So thats .00549 degrees per unit. Now I have not tested the total error, but I have seen it vary from expected by .015 degrees or so, but that is pretty close!

Sin and cos return between -30,000 and +30,000 for -1 and 1. One of the cool things about the cordic, is that it returns cos AND sin at the same time. I have heard that it is 90 times faster than math.h.

Also, this allows atan2 for a point x, y and accepts values from 0 to 32,767, and gives the hypotenuse to x,y which for me is the distance. It also gets these values at the same time!

This way my distance values should be spot on. I am still limited to about 200 miles between waypoints, the way I am measuring the change in x, and the change in y. But I could do a divide function to break the units into something it could handle. I just have not needed to do this for my application.

What I was using it for was navigation between waypoints. The first cordic for PIC16 did a pretty good job. It was accurate for bearing by about half a degree or so. My distance calculations did a simple square and square root that gave me errors on the hypotenuse. For long distances, I got pretty reliable results, less than 1 percent off of predicted results, but close range I could be off by a lot more. I could have cleaned this up with some higher math, but I was running out of speed trying to read two NMEA sentences, convert to decimal degrees for lat/lon, and fly a model plane, at 5 hertz gps readings. I gave up one sentence, and all is working well with the PIC16. This will allow me to break into the PIC18 world, and I am sure it will handle both NMEA sentences, plus a few more things. Nice!

[ScaleRobotics]

Here is the base working code for the PIC18 cordic. It is 600 bytes, and performs atan2, hypotenuse, sin, cos, and tan. Times will change with different angles and coordinate entries, but here are a few stats from Codetimer.bas: http://www.picbasic.co.uk/forum/cont...te-XYZ-command

sincos: Simulaneous result of SIN and COS
Time: 185.65924 usec
OSC Freq: 48 Mhz
-------------------------

atan: ATAN2 result as well as hypotenuse
Time: 178.32620 usec
OSC Freq: 48 Mhz
-------------------------

And just for comparison, here is some timing for using a slower chip and different cordic code, compared to math.h


And as a test, I ran the PBP code for SQR and ATN to compare it's speed.

x = x ATN y
y = SQR ang

took about 48.5 uS, but has much less precision.

PBP's SIN and COS function were much quicker at 4.16 uS, but again, much less precision than the cordic.

'/*******************************************************************************
'* FUNCTION NAME:    sin_cos
'*
'* ARGUMENTS:        int angle (angle in 16-bit binary radians)
'*
'* RETURNS:          x = sin, y = cos (and can give tan: remember tan=sin/cos)
'*
'* DESCRIPTION:      The angle is given in 16-bit radians (on a scale of -32,768
'*                   to 32,767). The function simultaneously calculates the sine
'*                   and cosine of the angle as fractions of 30,000 (where 30,000
'*                   equates to 1 and -30,000 equates to -1) and returns them in
'*                   a sin_cos_struct.
'*
'* EXAMPLE:          ang = 5461 'radians (30 degrees);
'*                   gosub sincos;
'*                   result: x = 15000, y = 25980 'radians 
'*                   For "decimal" divide by 3 to get 5000 (.5000) and 8660 (.8660)
'*                     To figure Tan result, use radians (x * 10000), then
'*                     tan = div32 y, result will be 5773 (for .5773) 
'*******************************************************************************/

'/*******************************************************************************
'* FUNCTION NAME:    atan2_sqrt
'*
'* ARGUMENTS:        int y (y-coordinate)
'*                   int x (x-coordinate)
'*
'* RETURNS:          x = atan2 y,x , or atan if x = 30000 radians (1.0000 decimal)
'*                     for atan: (atan2 y,1 = atan y)
'*                     y = hypotenuse 
'*                     atan2 or atan results will be in radians, see chart
'*
'* DESCRIPTION:      Given an ordered pair of coordinates, the function
'*                   simultaneously calculates the atan2 (the direction of the
'*                   position vector in 16-bit radians) and the square root of
'*                   the sum of the squares of the coordinates (the magnitude of
'*                   the position vector) and returns them in an
'*                   atan2_sqrt_struct.
'*
'* NOTES:            (1) The accuracy of the returned values increases as the
'*                   sizes of x and y increase. Consider multiplying both by a
'*                   scaling factor before calling the function.
'*                   (2) The function will fail for x and y values that result in
'*                   magnitues greater than 32,767 (the size of a signed int).
'*
'* EXAMPLE:          atan2_sqrt_struct bar;
'*                   int x = 25980, y = 15000;
'*                   gosub atan;
'*                   for the angle in radians: x = 5461 
'*                     for the hypotenuse: y = 30000
'*******************************************************************************/

;*******************************************************************************
; --- CORDIC TRIG LIBRARY ---
; http://www.chiefdelphi.com/media/papers/2016
; FILE NAME:        trig.inc
; AUTHOR:           Patrick Fairbank
; LAST MODIFIED:    FEB. 1, 2011 to make it cleaner 
;                   Modified by Walter Dunckel (Scale Robotics Inc.) with help from Darrel Taylor
;                   http://www.scalerobotics.com/cordic.html
; DESCRIPTION:      This file contains functions implementing the CORDIC
;                   algorithm, or how to get a 16 bit sin, cos, tan2 and hypotenuse result
;
; USAGE:            Add this file to your PicBasic Pro project using INCLUDE "TRIG.inc" 
;                   Then fill x,y values for atan2, or fill ang value for sincos
;                   then either GOSUB sincos or GOSUB atan 
; LICENSE:          Users are free to use, modify, and distribute this code
;                   as they see fit.
;
;******************************************************************************/
 
i       var byte     BANK0
j       Var byte     BANK0
quad    var byte     BANK0
x       var word     BANK0
y       var word     BANK0
ang     var word     BANK0
dy      var word     BANK0
dx      var word     BANK0
atans   var word[15] BANK0
 
atans(0) = 16384
atans(1) = 9672
atans(2) = 5110
atans(3) = 2594
atans(4) = 1302
atans(5) = 652
atans(6) = 326
atans(7) = 163
atans(8) = 81
atans(9) = 41
atans(10) = 20
atans(11) = 10
atans(12) = 5
atans(13) = 3
atans(14) = 1
 
goto OverAtan
 
sincos:
 
asm
  ; Initialize _x to 18218
  movlw 0x2a
  movwf _x
  movlw 0x47
  movwf _x+1
 
  ; Initialize _y to 0
  clrf _y
  clrf _y+1
 
  ; Initialize _quad to 0
  clrf _quad
 
  ; Check if the angle is greater than 16383 (90°)  
sc_check_greaterthan:
  btfss _ang+1, 7           ;*
  btfss _ang+1, 6           ;*
  bra sc_check_lessthan     ;
  bra sc_adjust_quad2       ;
 
  ; Check if the angle is less than -16384 (-90°)
sc_check_lessthan:
  btfsc _ang+1, 7            ;
  btfsc _ang+1, 6            ;
  bra sc_setup_end
 
  ; If the angle is in quadrant 3, adjust it to quadrant 4
sc_adjust_quad3:
  negf _ang                   ;
  bc sc_negate_quad3          ;
  comf _ang+1                 ;
  bra sc_adjust_end            
 
  ; If the low byte negation causes a carry, negate the upper byte
sc_negate_quad3:
  negf _ang+1
  bra sc_adjust_end
 
  ; If the angle is in quadrant 2, adjust it to quadrant 1
sc_adjust_quad2:
  comf _ang                   ;
  comf _ang+1                 ;
 
  ; Toggle the sign bit and set the '_quad' flag
sc_adjust_end:
  btg _ang+1, 7
  setf _quad
 
  ; Multiply the angle by 2 to get better resolution
sc_setup_end:
  bcf STATUS, 0
  rlcf _ang
  rlcf _ang+1
 
  ; Set up the main loop
sc_loop_start:
  clrf _i
  lfsr FSR0, _atans
 
    ; The main loop label
sc_loop:
    movff _x, _dy
    movff _x+1, _dy+1
    movff _i, _j
    movf _j
    bz sc_bs_x_done
 
      ; Loop to shift _dy right
sc_bs_x_loop:
      bcf STATUS, 0
      rrcf _dy+1
      rrcf _dy
      btfsc _x+1, 7
      bsf _dy+1, 7
      decfsz _j
      bra sc_bs_x_loop
 
    ; Calculate what needs to be added to _x
sc_bs_x_done:
    movff _y, _dx
    movff _y+1, _dx+1
    movff _i, _j
    movf _j
    bz sc_do_rotation
 
      ; Loop to shift _dx right
sc_bs_y_loop:
      bcf STATUS, 0
      rrcf _dx+1
      rrcf _dx
      btfsc _y+1, 7
      bsf _dx+1, 7
      decfsz _j
      bra sc_bs_y_loop
 
    ; Perform adding operations on _x, _y and _ang
sc_do_rotation:
    btfss _ang+1, 7
    bra sc_sub_angle
 
    ; If _ang is negative
    movf POSTINC0, W
    addwf _ang
    movf POSTINC0, W
    addwfc _ang+1
    movf _dx, W
    addwf _x
    movf _dx+1, W
    addwfc _x+1
    movf _dy, W
    subwf _y
    movf _dy+1, W
    subwfb _y+1
    bra sc_loop_bottom
 
    ; If _ang is positive
sc_sub_angle:
    movf POSTINC0, W
    subwf _ang
    movf POSTINC0, W
    subwfb _ang+1
    movf _dx, W
    subwf _x
    movf _dx+1, W
    subwfb _x+1
    movf _dy, W
    addwf _y
    movf _dy+1, W
    addwfc _y+1
 
    ; Increment the counter and exit the loop if done
sc_loop_bottom:
    incf _i
    movlw 0x0f
    cpfseq _i
    bra sc_loop
 
  ; Negate _x if it was initially in quadrant 2 or 3
sc_finished:
  btfss _quad, 7  ;
  bra sc_output   ;
  negf _x         ;
  bc sc_negate_x  ;
  comf _x+1       ;
  bra sc_output    
 
  ; If the low byte negation causes a carry, negate the upper byte
sc_negate_x:
  negf _x+1
 
  ; Output the calculated _x and _y values
sc_output:
 
endasm
 
return          'Done with sincos , return 
 
;######################################################################
; Calculates the magnitude and direction of the given ordered pair
;atan_sqrt:
 
 
atan:
asm
 
  ; Initialize _ang to 0
  clrf _ang
  clrf _ang+1
 
  ; Initialize _quad to 0
  clrf _quad
 
  ; If the point is in quadrant 2 or 3, make _x positive and set flag
as_check_negative:
  btfss _x+1, 7
  bra as_shift_x
  setf _quad
  negf _x
  bc as_negate_x
  comf _x+1
  bra as_shift_x
 
  ; If the low byte negation causes a carry, negate the upper byte
as_negate_x:
  negf _x+1
 
  ; Divide the _x coordinate by 2 to prevent overflowing
as_shift_x:
  bcf STATUS, 0
  rrcf _x+1
  rrcf _x
 
  ; Divide the _y coordinate by 2 to prevent overflowing
as_shift_y:
  bcf STATUS, 0
  rrcf _y+1
  rrcf _y
  btfsc _y+1, 6
  bsf _y+1, 7
 
  ; Set up the main loop
as_loop_start:
  clrf _i
  lfsr FSR0, _atans
 
    ; The main loop label
as_loop:
    movff _x, _dy
    movff _x+1, _dy+1
    movff _i, _j
    movf _j
    bz as_bs_x_done
 
      ; Loop to shift _dy right
as_bs_x_loop:
      bcf STATUS, 0
      rrcf _dy+1
      rrcf _dy
      btfsc _x+1, 7
      bsf _dy+1, 7
      decfsz _j
      bra as_bs_x_loop
 
    ; Calculate what needs to be added to _x
as_bs_x_done:
    movff _y, _dx
    movff _y+1, _dx+1
    movff _i, _j
    movf _j
    bz as_do_rotation
 
      ; Loop to shift _dx right
as_bs_y_loop:
      bcf STATUS, 0
      rrcf _dx+1
      rrcf _dx
      btfsc _y+1, 7
      bsf _dx+1, 7
      decfsz _j
      bra as_bs_y_loop
 
    ; Perform adding operations on _x, _y and _ang, shifting the _atans right one
as_do_rotation:
    movff POSTINC0, PRODL
    movff POSTINC0, PRODH
    bcf STATUS, 0
    rrcf PRODH
    rrcf PRODL
    btfsc  _y+1, 7
    bra as_sub_angle
 
    ; If _y is positive
    movf PRODL, W
    addwf _ang
    movf PRODH, W
    addwfc _ang+1
    movf _dx, W
    addwf _x
    movf _dx+1, W
    addwfc _x+1
    movf _dy, W
    subwf _y
    movf _dy+1, W
    subwfb _y+1
    bra as_loop_bottom
 
    ; If _y is negative
as_sub_angle:
    movf PRODL, W
    subwf _ang
    movf PRODH, W
    subwfb _ang+1
    movf _dx, W
    subwf _x
    movf _dx+1, W
    subwfb _x+1
    movf _dy, W
    addwf _y
    movf _dy+1, W
    addwfc _y+1
 
    ; Increment the counter and exit the loop if done
as_loop_bottom:
    incf _i
    movlw 0x0e
    cpfseq _i
    bra as_loop
 
  ; Multiply the _x value by 19898 and divide by 2^14 to scale it
as_scale_x:
  movff _x, _dx
  movff _x+1, _dx+1
  movlw 0xba
  mulwf _dx
  movff PRODH, _x
  movlw 0x4d
  mulwf _dx+1
  movff PRODH, _dy
  movff PRODL, _x+1
  movlw 0xba
  mulwf _dx+1
  movf PRODL, W
  addwf _x, F
  movf PRODH, W
  addwfc _x+1, F
  clrf WREG
  addwfc _dy, F
  movlw 0x4d
  mulwf _dx
  movf PRODL, W
  addwf _x, F
  movf PRODH, W
  addwfc _x+1, F
  clrf WREG
  addwfc _dy, F
  movlw 0x06
  movwf _j
as_scale_bs_loop:
    bcf STATUS, 0
    rrcf _dy
    rrcf _x+1
    rrcf _x
    decfsz _j
    bra as_scale_bs_loop
 
  ; Check if the quadrant was originally changed
as_check_quad:
  btfss _quad, 7
  bra as_output
  btfss _ang+1,7
  bra as_adjust_quad1
 
  ; If the angle is in quadrant 4, adjust it to quadrant 3
as_adjust_quad4:
  negf _ang
  bc as_negate_quad4
  comf _ang+1
  bra as_adjust_end
 
  ; If the low byte negation causes a carry, negate the upper byte
as_negate_quad4:
  negf _ang+1
  bra as_adjust_end
 
  ; If the angle is in quadrant 1, adjust it to quadrant 2
as_adjust_quad1:
  comf _ang
  comf _ang+1
 
  ; Toggle the sign bit
as_adjust_end:
  btg _ang+1, 7
 
  ; Output the calculated angle and hypotenuse values
as_output:
endasm
 
return
 
OverAtan:

[ScaleRobotics]

Added this content to post 13

[Archangel]

WOW, I have enough trouble doing trig on a calculator. Thank You both for this. Could you please assist us mathematicly challenged with a couple of lines of example code as to how to use this masterpiece? I get the part where my program has main: respondant to your goto main, but how do I use the functions? Or is that what post 8 is about?

[bikxsici]

hi ScaleRobotics , i am interested in your codes posted on #51. and i have a problem.
...
ang=3000
call sincos
x=60395, y=39551
...
for all ang values i get same numbers, i dont understan what is the problem? thank you

[ScaleRobotics]

Thanks Martin,

That works! But there is a little less math in post 49. It makes the rest of the cordic do what its supposed to for the rest of the quadrants. Thanks for pointing it out.

Here is TRIG.inc V1.5

[mark_s]

Walter,

Thank you for your original work and the update.

Martin, thanks for bringing up again.

I was trying to make this work last year. My results were similar to Martin's.
Assuming it was something in my code. I had to settle for pbp ATN.
Can't wait to try it out

Best regards

[readitaloud]

Walter,,

Yes the statement: IF ang > 9000 then ang = 45000 - ang, does the job. For some reason the first time I tried using it the results were crazy so I simply tried to make a patch. I must have typed in something incorrectly. Excellent job Walter ! Thanks for this piece of code.

73's de Martin

[ScaleRobotics]

Hmmm,

Well, I can't test it from here, but this seems like it might work. (If not, I will think harder and do some testing). Thanks for the feedback Martin.

sincos:
    'use angle as deg.dd for example 35999 is 359.99 degrees
    if ang < 9001 then ang = 9000 - ang              'change degrees to radians
    if ang > 9000 then ang = 45000 - ang            'change degrees to crazy radians
    ang = ang * 466
    ang = div32 256

 asm
        call sin_cos
endasm
return

[readitaloud]

Walter,

I added three lines as shown in red. With the additions all 4 quadrants are returning correct magnitude and sign values.

See attached test file.

- Martin


;************************************************* *****************************/


i var byte BANK0
j Var byte BANK0
quad var byte BANK0
x var word bank0
y var word bank0
ang var word bank0
ang_temp var word bank0
dy var word bank0
dx var word bank0
atans var word[15] bank0


atans(0) = 16384
atans(1) = 9672
atans(2) = 5110
atans(3) = 2594
atans(4) = 1302
atans(5) = 652
atans(6) = 326
atans(7) = 163
atans(8) = 81
atans(9) = 41
atans(10) = 20
atans(11) = 10
atans(12) = 5
atans(13) = 3
atans(14) = 1


goto overcordic


atan2:
asm
call atan2_sqrt
endasm
'convert to degrees.dd y is degrees
If ang < 16384 then y = 16383 - ang
if ang > 16383 then
y = 65535 - ang
y = y + 16383 'correct 90 degrees for radian
endif
y = y * 256 'divides radians to get degrees within 57ppm
y = div32 466 'degrees.dd is y, radians is ang

return


sincos:
'use angle as deg.dd for example 35999 is 359.99 degrees
ang_temp = ang ' Store ang.
if ang < 9001 then ang = 9000 - ang
if ang > 9000 then ang = 27000 - (36000 - ang)
ang = ang * 466
ang = div32 256TRIG.basTrig_example.bas

asm
call sin_cos
endasm
if ang_temp > 9000 then y = ~y + 1 ' Perform 2's complement of y if ang > 90 for cos(ang).
return


asm


; Calculates the sine and cosine of the given angle
sin_cos: