' Documented Cordic
'
' A vector from the origin to a point may be represented as X and Y co-ordinates. And those co-ordinates
' if the vector is of unity length are equal to the sin and cosines of the angle between the x axis and the point.
'
' x = cos(Theta)
' y = sin(Theta)
'
' So we can see that if we find values of x and y corresponding to a given theta we have sin and cos(theta)
' and if we find the change in theta required to reduce y to zero we have found the angle theta. The CORDIC
' algorithm does just that.
'
'**********************************************************************************************************************
'
' --- Performing a sin(theta) or Cos(theta), general description
'
' What we have are two vectors, one is the vector we want to determine the angle of or the x and y
' co-ordinates of (to get sine and cos) and the other is our estimate vector with co-ordinates x'and y'.
'
' In the case of finding Cos or Sin(theta) we start with our estimate vector on the x axis, it is then rotated by 45
' degrees towards the vector to be measured. 45 degrees is added to the angle accumulator and the values for
' x' and y' are modified. How do you do that? How can we work out where the end of the estimate vector is just by the
' angle when we can't do sin/cos? The trick is to choose the angles very carefully so we know what the change in x'
' and y' will be, more on that in the maths. The important word is change, we have to go thorough a process of tweaking
' the values of x' and y', estimate by estimate until they are correct.
'
' Say for instance that the angle we are trying to get the sin/cos of is +30 degrees, our rotation of the
' estimate vector has overshot. So the next rotation must be in the opposite direction. How big is the rotation?
' well it is smaller than before, 26.56 degrees, seems random but its not as we will see. So after this rotation
' the angle accumulator has 18.44 degrees in it and the values of x' and y' have been updated too, next the angle
' to be rotated is 14 degrees and the rotation direction is positive again. Now the angle accumulator
' has 18.44+14 = 32.44 degrees, getting warmer and the values in x' and y' are getting closer to sin(theta) and
' cos(theta) too.
'
' This process continues for a fixed number of iterations, the angle of rotation gets smaller each time as the correct
' angle is homed in on. At the end of the process you have your values of x' and y' and hence your sin(theta)
' and cos(theta) values.
'
'
'**********************************************************************************************************************
'
' --- Getting the angle - General description.
'
' You can do a very similar thing to find the angle given the x and y co-ordinates and this is what the assembly
' code below does. To find an angle the values of x' and y' are set to those provided. The angle accumulator
' is set to zero and a similar process ensues. The difference in this case is that rather than aiming for a target
' vector the algorithm is trying to rotate the estimate vector to the x-axis, once it has done that the angle
' accumulator will contain the correct angle. As before it is performed rotation by rotation, the sign of y'
' determines the direction of rotation if it becomes positive the vector must be rotated CW and if negative CCW
' to make sure it moves towards the x-axis. So although not used directly the values of x' and y' are still
' important in deciding if to add or subtract the increasingly small rotation angles to the angle estimate.
'
'
'**********************************************************************************************************************
'
' --- Some maths
'
' As we know a vector can be represented by a pair of co-ordinates x,y
'
' If that vector is rotated around the origin by an angle theta the new co-ordinates of its endpoint become
'
' x' = x cos(theta) - ySin(theta) ' From some trig (look up on net if interested)
' y' = y cos(theta) + xSin(theta)
'
' As Tan(theta) = Sin(theta)/Cos(theta) then it may be re written as
'
' x' = cos(theta)( x - yTan(theta))
' y' = cos(theta)( y + xTan(theta))
'
' This looks a little worse if anything, we now have a Tan multiplied by y. If only yTan(theta) could be easily
' calculated. Well in binary, multiplies/divides by a power of 2 (2,4,8...) can be done by bit shifting. To multiply by 2 you
' simply shift the bits 2^1 places to the left, 0010 => 0100 voila! Similarly you can divide with left bit shifts so if Tan(theta)
' was made a power of 2 the yTan(theta) could be done with simple shifts.
'
' As binary itself shows us you can represent a number by adding up lots of progressively smaller bits so this is what
' is done in the Cordic algorithm. We choose Tan(theta) to be a limited set of powers of 2, to be precise we set:
'
' Tan(theta) = 2^-i (note: 2^-i means 1/(2^i) and i is the iteration number,)
'
' when i = 1 Tan(theta) = 2^-1 and theta = 45 degrees
' when i = 2 Tan(theta) = 2^-2 and theta = 26.56 degrees
' ....
'
' Sound familiar?
'
' We can easily put these angles in a look up table and access them as we go along, the example below
' uses 20 of them, that's not many for some amazing accuracy.
'
' Doing some more mathematical substitution and defining Xi as the current estimate and Xi+1 as the next
' we have:
'
' Xi+1 = Cos(atan(2^-i)).[Xi - YiDi(2^-i)]
' Yi+1 = Cos(atan(2^-i)).[Yi + XiDi(2^-i)]
'
' Its not as bad as it looks. Firstly I have dropped the ' for clarity and secondly atan(2^-i) has been subbed for
' theta because that is the angle we shall be rotating by. There is also a new variable called Di, I suppose that D
' might stand for decision as this value is determined by the direction of rotation required and may be +/-1.
'
' The whole thing can be neatened up by defining:
'
' Ki = Cos(atan(2^-i))
'
' Xi+1 = Ki.[Xi - YiDi(2^-i)]
' Yi+1 = Ki.[Yi + XiDi(2^-i)]
'
' Here's the clever part, Ki does not contain anything that varies other than i, and i always does the same thing,
' it goes from 1 : n. The thing to understand is that without Ki the estimate vector gets longer and longer and
' will end up being longer than one, that will mean that Xi and Yi will no longer represent Sine and Cosine of
' Theta. Ki simply scales the vector to unity and it does the same thing every time you run the cordic
'
' The final effect of Ki is that the result has been multiplied by K1*K2*K3*K4 ...... Kn
'
' This may be given the name K (the cordic gain) and applied to Xi and Yi eiher after or before iterating
' ___________________________________________________________________________________________________
'| |
'| Aside: K, the Cordic gain |
'| |
'| K = Product(Ki) from K1-Kn ' This means K1*K2*Kn... |
'| |
'| K = Product(Cos(atan(2^-1))) ' Subbing for Ki |
'| |
'| K = Product(1/sqrt(1+2^-2i)) ' according to some boffin |
'| |
'| and that becomes 0.607252935 very quickly as n gets bigger (because cos of a small angle is one). |
' ____________________________________________________________________________________________________|
' So cutting a long story short you either multiply X and Y by 0.607252935 at the beginning or at the end if you
' care about there real lengths.
' So now we have the more dainty equations of:
'
' Xi+1 = [Xi - YiDi(2^-i)]
' Yi+1 = [Yi + XiDi(2^-i)]
'
' A reminder on how to use them.
'
' To find sin/cos:
'
' 1. Begin with Xi = 1 and Yi = 0 so the estimate is on the x-axis
' 2. Iterate the equation, updating the values of Xi and Yi, Increment/decrement the angle accumulator according to Di
' with the values in the look up table, 45, 26.5 etc. Work out the next value for D by comparing the the angle accumulator
' with the angle provided (is the estimate less or greater than the angle).
' 3. Repeat 2 for a given number of steps, more steps means greater accuracy.
' 4. Scale Xi and Yi by 0.607252935
' 5. And relax
'
' To find theta given x and y:
'
' 1. Begin with Xi = x and Yi = y , the values given.
' 2. Iterate the equation, updating the values of Xi and Yi. Increment/decrement the angle accumulator with the values in the
' look up table, 45, 26.56 etc decide on Di by looking at sign of Yi in order to rotate the estimate towards the x-axis.
' 3. Repeat 2 for a given number of steps
' 4. Relax, the angle accumulator holds the answer no need for scaling.
'
'
'******************************************************************************************************************
' And now some assembly code:
'
'
' This is code taken from the mesmic2125 object, it computes angle given x any y values. I have added my comments.
' Comments between lines tend to be explanatory of the process, those on the lines are about the code and may help
' those new to assembly (like me)
DAT 'cx and cy are the supplied variables.
'ca will hold the eventual answer
cordic ' The initial vector could be in any of the 4 quadrants, the basic algorithm will only work for angles between
' +/- 90 degrees but you want to be able to get an answer in any instance. You also want to ensure that the final
' answer is between 360 degrees.
'
' The number system for angle is carefully chosen, it varies from 0-360 or rather it varies from 0-0, as it overflows
' back to 0 when full. This means that if your starting vector is in the 4th quadrant (down and right) and is say
' 10 degrees from the x-axis if the angle is set to zero degrees at the start then as the estimate approaches the
' x-axis the angle becomes 340.
' ________________________________________________________________________________________________________________
'| |
'| Why? Its 2s-complement, |
'| |
'| take an 8 bit number 00000000 then take one from it you get 11111110 which if you forget about the fact it |
'| should be neagitive is 254 or more importantly 255-1 |
'| |
'| As a further note, in assembly the program has to keep track of whether a number is supposed to be signed |
'| or not. e.g. the first line of this code checks for negativeness (wc)so we must assume cx is being used as a |
'| signed variable. |
'|________________________________________________________________________________________________________________|
' If the vector is in the 1st quadrant the angle is just added as the estimate vector moves towards the x-axis.
'
' If in the 3rd quadrant (left and down) the angle is set to 180 at the start and the vector flipped to the 1st quadrant.
' This means that the final angle is 180 + the swept angle as it should be.
'
' Finally for the 2nd quadrant the angle is set to 180 at the start and the vector flipped to the 4th quadrant the final result
' becomes 180-swept angle.
'
' Make cx positve as the algoritm always operates in quadrants 1 and 2.
' This makes Q2=>Q1 and Q3=>Q4
abs cx,cx wc ' take the absolute of cx
' If was in Q3 or Q2 flip about x-axis
' So Q2=>Q4 and Q3=>Q1 overall.
if_c neg cy,cy ' negate cy if cx was negative
' Decide on starting angle, 0 or 180
mov ca,#0 ' load 0 into ca, the estimated angle
rcr ca,#1 ' Load with 180 if was on left
movs :lookup,#table ' Puts address of table into sumc command
mov t1,#0 ' Initialize t1 (angle shift 2^-t)
mov t2,#20 ' Initialize t2 (number of interations)
'The loop
:loop 'First calculate dX(i+1) => Yi.2^-t
mov dx,cy wc ' Load dx with cy cary is set if negative
sar dx,t1 ' Shifts dx by t1 while preserving sign
'Then calculate dY(i+1) => Xi.2^-t
mov dy,cx ' Load dy with cx
sar dy,t1 ' Shifts dy by t1 while preserving sign
'Now add to the estimate with sign dependant on sign of Yi
sumc cx,dx ' Add -dx to cx if c=1 or dx if c=0
sumnc cy,dy ' Add -dy to cx if c=0 or dy if c=1
'Next add or subract the specified rotation from the angle accumulator
:lookup sumc ca,table ' Add -table to ca if c=1 or table if c=0
' Remembering that the source field of the sumc command "table"
' is being over written to address different elements
' of the table below
add :lookup,#1 ' Increments the destination of lookup sumc command
add t1,#1 ' increment t1, this is i
djnz t2,#:loop ' decrement t2, keep looping if not zero.
cordic_ret ret ' return to rest of program (whatever it is)
'Table of atan(2^-i)
table long $20000000 'atan(1) = 45 degrees
long $12E4051E 'atan(1/2) = 26.56 degrees
long $09FB385B 'atan(1/4)
long $051111D4 'atan(1/8)
long $028B0D43 'atan(1/16)
long $0145D7E1 'atan(1/32)
long $00A2F61E 'atan(1/64)
long $00517C55 'atan(1/128)
long $0028BE53 'atan(1/256)
long $00145F2F 'atan(1/512)
long $000A2F98 'atan(1/1024)
long $000517CC 'atan(1/2048)
long $00028BE6 'atan(1/4096)
long $000145F3 'atan(1/8192)
long $0000A2FA 'atan(1/16384)
long $0000517D 'atan(1/32768)
long $000028BE 'atan(1/65536)
long $0000145F 'atan(1/131072)
long $00000A30 'atan(1/262144)
long $00000518 'atan(1/524288) = 1e-4 degrees
' Uninitialized data
t1 res 1
t2 res 1
dx res 1
dy res 1
cx res 1
cy res 1
ca res 1