 |
 |
| View previous topic :: View next topic |
| Author |
Message |
Douglas Kennedy
Joined: 07 Sep 2003
Posts: 755
Location: Florida
|
| CORDIC 32bit Functions |
 Posted: Wed Aug 16, 2006 9:11 am |
 |
|
This calculates sine cosine arcsin arccos sqrt(x^2+y^2) to 7 decimal places without the use of floating point.
Does polar to cartesian and the reverse translations and rotation of cartesian coordinates thru an angle.
Note
sine and cosine are shown as seperate calls but they maybe derived together within the same iterative process.
| Code: |
#include <18F452.h>
#DEVICE *=16 ICD=TRUE
//#include <cordic.h>
#fuses HS,NOWDT,NOPROTECT,NOPUT,NOLVP,NOBROWNOUT,NOWRT
#use delay(clock=20 000 000)
//#use rs232(baud=57600, bits=8, parity = N, xmit=PIN_C6, rcv=PIN_C7)
#use rs232(DEBUGGER)
#include <cordic32.h>
/// special macros /////////////////////////////////////////////
/// #x id the litteral value of the variable x
/// suppose x is variable reboot then #x is the litteral "reboot"
#define debug(x) printf("%s is %ld\r\n",#x,x);
main()
{
signed int32 result,x,y;
/// 45 degs is 45 0 000 000
debug(cos(450000000));
debug(sin(600000000));
printf("arctan(6)"); // 6=600,000,000/100,000,000
debug(arctan(100000000,600000000));
printf("sqrt of (6^2+10^2)/.607252935\n\r");
debug(sqrt_x2_y2(100000000,60000000));
x=607252935; // this is 1 prescaled
y=607252935; // this is 1 prescaled
rot(&x,&y,600000000); /// axis thru 60 degs
printf("x=%ld y=%ld \n\r",x,y);
x=607252935; /// prescaled unit vector magnitude
y=600000000; /// phase angle in degs
polartocart(&x,&y);
printf("x=%ld y=%ld \n\r",x,y);
x=607252935; /// prescaled to give unit vector
y=607252935; /// prescaled to give unit vector
carttopolar(&x,&y);
printf("x=%ld y=%ld \n\r",x,y);
debug(arcsin(800000000));
debug(arccos(800000000));
while(true);
}
######### here is the cordic32.h file ############
//////////// cordic ///////////////////////////////////
////////////////////////////////////////////////////////////////
/// theory /////
/// /////
/// coords (x,y) are rotated thru angle a to (x',y') /////
/// /////
/// x'=x.cos(a)-y.sin(a) /////
/// y'=y.cos(a)+x.sin(a) /////
/// /////
/// x'/cos(a)=x-y.tan(a) /////
/// y'/cos(a)=y+x.tan(a) /////
/// /////
/// now if we choose tan(a) to be /////
/// 1/2,1/4,1/8......1/2*n /////
/// /////
/// we can rotate thru sucessive values /////
/// ex 30.00 degs = 45-26.57+14.03-7.13+3.58 ////
/// +17.9,.90,.45 ////
/// where a minus angle is a counter rotation ////
/// after a rotation of 30 degs ////
/// x' is x.cos(30) if we start with y=0 ////
/// y' is x.sin(30) ////
/// ////
///angle is in 1/10000000th units 450000000 is 45 degrees ////
/// lengths are in aggregate constant units ////
/// ////
/// aggregate const cos(atan(1)*cos(atan(1/2)*cos(atan(1/4).....)))))
/// or .607252935 ////
/// so 1000000000 is 607252935 for 0.6072529350088813
/// this will give 7 digit accuracy ////
///
/// Example
// main()
// {
// signed int32 result;
// result= cos(600000000);
// printf("cosine of 60 is %ld \n\r",result);
// result= sin(600000000);
// printf("sine of 60 is %ld \n\r",result);
// result= arctan(100000000,60000000);
// printf("arctan of 0.60 is %ld \n\r",result);
// result=sqrt_x2_y2(100000000,60000000);
// printf("sqrt of (6^2+10^2)/.607252935; is %ld \n\r",result);
// while(true);
// }
//////////////////////////////////////////////////////////////
int32 const atan[30]={450000000,265650512,140362435,71250163,35763344,17899106,8951737,4476142,
2238105,1119057,559528,279765,139882,69941,34971,17485,8742,4371,
2186,1093,546,273,136,68,34,17,9,4,2,1};/// atan(1/2n)
/// cos(atan(1/2^i))=1/sqrt(1+1/2^2i)
/// there is a gain on each iteration of sqrt(1+2^-2i) eg i=1 sqrt(1.25)
/// we use the reciprocal of the gain which converges to 607252935
/// gan is the accumulated multiplicative gain at each iteration
//int32 const gain[15]={707106781,632455532,613571991,608833912,607648256,607351770,607277644,607259112,
// 607254479,607253321,607253031,607252959,607252941,607252936,607252935} ;
////
////
//// to clac sin(A) cos(A) set y=0 a=angle and drive a to zero
//// via iterations
//// x value is the cos(a) providing x=607252935
//// y value is the sign(a) providing x=607252935
////
//// to calc atan(a) set a=0 and drive y to zero
//// a value is atan(y/x)
//// x value is sqrt(x^2+y^2) providing x=607252935
signed int32 arccos(signed int32 c)
{
int i;
signed int32 a,x,y,dx,dy,da;
y=607252935;
x=0;
a=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (c<x)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
a= 900000000-a;
return (a);
}
signed int32 arcsin(signed int32 c)
{
int i;
signed int32 a,x,y,dx,dy,da;
x=607252935;
y=0;
a=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (y<c)
{
a=a+da;
x=x-dy;
y=y+dx;
}
else {
a=a-da;
x=x+dy;
y=y-dx;
}
}
return (a);
}
carttopolar(signed int32* xptr,signed int32* yptr)
{
int i;
signed int32 a,x,y,dx,dy,da;
// x is transfomed to magnitude sqrt(x^2+y^2)
// y is transformed to arctan(y/x)
x=*xptr; // get value pointed to by ptrmag
y=*yptr; // get value pointed to by phaseptr
a=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (y<0)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
*xptr=x;
*yptr=a;
}
polartocart(signed int32* magptr,signed int32* phaseptr)
{
int i;
signed int32 a,x,y,dx,dy,da;
// magnitude is transfomed to x'=r*cos(a)
// phase is transformed to y'=r*sin(a)
// x' and y' need to be scaled by .607252935
x=*magptr; // get value pointed to by ptrmag
a=*phaseptr; // get value pointed to by phaseptr
y=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (a>=0)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
*magptr=x;
*phaseptr=y;
}
void rot(signed int32* ptrx,signed int32* ptry,signed int32 a)
{
int i;
signed int32 x,y,dx,dy,da;
// coords x,y are transformed to x' y' by counter clockwise rotation thru a degrees
// x'=x*cos(a)-y*sin(a) y'=x*sin(a)+y*cos(a)
// x' and y' need to be scaled by .607252935
x=*ptrx; // get value pointed to by ptrx
y=*ptry; // get value pointed to by ptry
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (a>=0)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
*ptrx=x;
*ptry=y;
}
signed int32 cos(signed int32 a)
{
int i;
signed int32 x,y,dx,dy,da;
x=607252935;
y=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (a>=0)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
return (x);
}
signed int32 sin(signed int32 a)
{
int i;
signed int32 x,y,dx,dy,da;
x=607252935;
y=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (a>=0)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
return (y);
}
signed int32 arctan(signed int32 x,signed int32 y)
{
int i;
signed int32 a,dx,dy,da;
a=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (y<0)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
return (a);
}
signed int32 sqrt_x2_y2(signed int32 x,signed int32 y)
{
int i;
signed int32 a,dx,dy,da;
a=0;
for (i=0;i<30;i++)
{
if (X>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;}
if (Y>=0) {dy=Y >>i;} else { dy=-Y >> i;dy=-dy;}
da=atan[i];
if (y<0)
{
a=a-da;
x=x-dy;
y=y+dx;
}
else {
a=a+da;
x=x+dy;
y=y-dx;
}
}
return (x);
}
|
Last edited by Douglas Kennedy on Tue Aug 28, 2007 5:13 am; edited 1 time in total |
|
 |
Douglas Kennedy
Joined: 07 Sep 2003
Posts: 755
Location: Florida
|
| Very possible BUG in posting code on this board |
| Posted: Fri Aug 18, 2006 4:40 am |
|
|
The expression (i=0;i<30;i++) will show up as (i=0;i<30>0)
It is probably because I haven't disabled something prior to posting optomistically expecting the code commands would actual allow valid CCS code to be accepted |
|
|
AdamkT1
Joined: 21 Apr 2007
Posts: 44
|
| Problem in code |
| Posted: Mon Aug 27, 2007 11:27 am |
|
|
Hi,
I have been looking for cordic.
Seems problem with the code sent.
I think it is in every for loop:
| Quote: | | for (i=0;i<30>=0) {dx=X >>i;} else { dx=-X >> i;dx=-dx;} |
I think it is missing if condition.
I am particularly looking for arccos... |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Mon Aug 27, 2007 11:57 am |
|
|
See the "sticky" post at the top the code forum which explains this problem.
I have tried to get Admin to disable HTML by default for the whole board,
but have been unsuccessful in my requests. So, you have do it for each
post, or better, edit your user profile to disable it. |
|
|
Douglas Kennedy
Joined: 07 Sep 2003
Posts: 755
Location: Florida
|
|
| Posted: Tue Aug 28, 2007 5:24 am |
|
|
| HTML was off in my profile yet my post to this code section still has translation errors. Disabling HTML prior to posting appears to be the only way i have found to make it work. |
|
|
AdamkT1
Joined: 21 Apr 2007
Posts: 44
|
| ArcCos function |
| Posted: Tue Aug 28, 2007 9:02 am |
|
|
| Is the ArcCos function tested... |
|
|
Douglas Kennedy
Joined: 07 Sep 2003
Posts: 755
Location: Florida
|
|
| Posted: Tue Aug 28, 2007 2:45 pm |
|
|
AdamkT1 asks is this tested?
This is a self help board not a help yourself board. The above code is not warranted in anyway. The code works in the first quadrant if you don't understand what this means or don't understand the CORDIC algorithm then write arcos on your own. You must test this code yourself....if you understand trig then you can independently verify the results but at all times you are absolutely responsible for its use especially for navigation. If you have any concerns then please don't use it. |
|
|
AdamkT1
Joined: 21 Apr 2007
Posts: 44
|
|
| Posted: Wed Aug 29, 2007 9:43 am |
|
|
I did not mean any offence to anybody, honestly, because I simply can’t afford to do that as this forum has been the only productive one for me, so far.
Actually, before the corrections were done into in the code, I had gone through a lot of info regarding CORDIC Arc functions. The extracts were that for arc functions double iterations must be done in order to reduce the error for angles reaching 90 degrees.
The base line code I used was the code of this forum, Sin & Cos functions were performing well but the Arc function (I only tested Arcos) was not behaving properly.
That is why I had to ask the question in that silly manner. I am sorry for that.
Besides I had replies from one person so there was no reason for me to be rude to that only person.
I hope it clarifies my position. |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Wed Aug 29, 2007 2:09 pm |
|
|
Adam,
He's really just saying that the Cordic code is posted "as is", and he
doesn't want to provide detailed and continuous support for it.
One of the "risks" of posting code in the Code Library is an expectation
by users that the author will provide support for the code. Not everyone
wants to do this. And that's fair. Some people want to help, but they
don't want to be "drafted" into doing a lot of support. |
|
|
Douglas Kennedy
Joined: 07 Sep 2003
Posts: 755
Location: Florida
|
|
| Posted: Tue Sep 04, 2007 1:17 pm |
|
|
All power series have areas of convergence.Ex 1/ (1-x^2)
Taylor series expands to 1+x^2+x^4+x^6+x^8 ........
for x=1/2 we get 1/(1-1/4) = 4/3 and the expansion converges to this number. But if x=2 we get -1/3 and the expansion doesn't work. Some will say this is an error but really the answer lies in the definition of smooth functions. Fortunately trig functions are smooth and converge in the unit circle in the complex plane with numbers expressed in the complex form e^i a where e is Eulers function i is the sqrt(-1) and a the angle in radians. CORDIC iteration due to the fact it is cut off at 32 bits isn't mathematically smooth however e^2ia =(e^ia)^2 yields since e^ia=cos(a)+i*sin(a) the equation cos(2a)=(cos(a))^2 - (sin(a)^2). So with some trig and multiplication you can use half the angle to get values when using the full angle accumulates an error you don't wish to accept. |
|
|
Andrei
Joined: 26 Aug 2007
Posts: 3
|
|
| Posted: Thu Jun 05, 2008 2:06 pm |
|
|
| Could anybody give me an example how to implement this algorithm into calculating distance between two GPS positions? |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
