Subversion Repositories kvcordic

//
// Filename: cordic.c
// Purpose : N-address code (NAC) implementation for a fixed-point universal 
//           CORDIC. The arithmetic representation used is signed fixed-point 
//           (Q2.14S). The implementation has been generated by a modified 
//           version of the simple fixed-point CORDIC tools from:
//           http://www.dcs.gla.ac.uk/~jhw/cordic/
// Author  : Nikolaos Kavvadias (C) 2010
// Date    : 31-Oct-2010
// Revision: 0.3.0 (31/10/10)
//           Initial version.
//           0.3.1 (08/11/10)
//           Description of the universal CORDIC algorithm is finalized.
// 
#include <stdio.h>
#include <math.h>
 
#define ROTATION   0
#define VECTORING  1
#define CIRCULAR   0
#define LINEAR     1
#define HYPERBOLIC 2
 
typedef short int integer;
//typedef integer int;
 
//Cordic in 16 bit signed fixed point math
//Function is valid for arguments in range -pi/2 -- pi/2
//for values pi/2--pi: value = half_pi-(theta-half_pi) and similarly for values -pi---pi/2
//
// 1.0 = 16384
// 1/k = 0.6072529350088812561694
// pi = 3.1415926536897932384626
//Constants
#define MY_PI 3.1415926536897932384626
// Q2.14S
#define cordic_1K 9949
#define cordic_1Kp 19783
#define half_pi 25735
#define MUL 16384.000000
#define CORDIC_NTAB 14
integer cordic_tab[3*CORDIC_NTAB] = {
  65535 /* NOT USED */, 8999, 4184, 2058, 1025, 512, 256, 128, 64, 32, 16, 8, 4, 2, /* HYPERBOLIC */
  16384, 8192, 4096, 2048, 1024, 512, 256, 128, 64, 32, 16, 8, 4, 2,                /* LINEAR */
  12867, 7596, 4013, 2037, 1022, 511, 255, 127, 63, 31, 15, 7, 3, 1                 /* CIRCULAR */
};
// for convergence in hyperbolic mode, steps 4 and 13 must be repeated
integer cordic_hyp_steps[] = {
//	1, 2, 3, 4, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28
	1, 2, 3, 4, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 13
};
integer gdirection; // {0: ROTATION, 1: VECTORING}
integer gmode; // {0: CIRCULAR, 1: LINEAR, 2: HYPERBOLIC}
 
void cordicopt(integer direction, integer mode, integer xin, integer yin, integer zin, integer *xout, integer *yout, integer *zout)
{
  integer k, kk, d, x1, x2, y1, y2, z1, z2;
  integer x, y, z;
  integer kstart, kfinal, xbyk, ybyk, tabval;
  integer offset;
 
  x = xin;
  y = yin;
  z = zin;
  offset = ((mode == HYPERBOLIC) ? 0 : ((mode == LINEAR) ? 14 : 28));
  kfinal = ((mode != HYPERBOLIC) ? CORDIC_NTAB : CORDIC_NTAB+1);
  for (k = 0; k < kfinal; k++)
  {
    d = ((direction == ROTATION) ? ((z>=0) ? 0 : 1) : ((y<0) ? 0 : 1));
    kk = ((mode != HYPERBOLIC) ? k : cordic_hyp_steps[k]);
    xbyk = (x>>kk);
    ybyk = ((mode == HYPERBOLIC) ? -(y>>kk) : ((mode == LINEAR) ? 0 : (y>>kk)));
    tabval = cordic_tab[kk+offset];
    x1 = x - ybyk;
    x2 = x + ybyk;
    y1 = y + xbyk;
    y2 = y - xbyk;
    z1 = z - tabval;
    z2 = z + tabval;
    x = ((d == 0) ? x1 : x2);
    y = ((d == 0) ? y1 : y2);
    z = ((d == 0) ? z1 : z2);
  }  
  *xout = x;
  *yout = y;
  *zout = z;
}
 
int main(int argc, char **argv)
{
  double p;
  integer x1, y1, z1, x2, y2, z2, i, temp;   
  integer w1;
 
  // ROTATION, SIN/COS
#ifndef DATAGEN
  printf("SINE, COSINE\n");
#endif
  gdirection = ROTATION; gmode = CIRCULAR;
  x1 = cordic_1K; y1 = 0;
  for (i = 0; i < 50; i++)
  {
    p = (i/50.0)*MY_PI/2;        
    z1 = (integer)(p * MUL);
    cordic(gdirection, gmode, x1, y1, z1, &x2, &y2, &z2);
#ifdef DATAGEN    
// Use this to generate the "cordic_test_data.txt" reference vectors.
    printf("%04x %04x %04x %04x %04x %04x %04x %04x\n", 
      gdirection, gmode, x1&0xffff, y1&0xffff, z1&0xffff, x2&0xffff, y2&0xffff, z2&0xffff);
#else
// Use this to compare floating-point (math.h) against CORDIC fixed-point
// results.
    printf("%f : cos=%f (%f), sin=%f (%f)\n", p, x2/MUL, cos(p), y2/MUL, sin(p));
#endif    
  }       
 
  // VECTORING, ATAN
#ifndef DATAGEN
  printf("\nARCTAN\n");
#endif
  gdirection = VECTORING; gmode = CIRCULAR;
  z1 = 0;
  for (x1 = 0; x1 <= 7000; x1+=500)
  {
    for (y1 = 0; y1 <= 7000; y1+=500)
    { 
      cordic(gdirection, gmode, x1, y1, z1, &x2, &y2, &z2);
#ifdef DATAGEN    
// Use this to generate the "cordic_test_data.txt" reference vectors.
    printf("%04x %04x %04x %04x %04x %04x %04x %04x\n", 
      gdirection, gmode, x1&0xffff, y1&0xffff, z1&0xffff, x2&0xffff, y2&0xffff, z2&0xffff);
#else
// Use this to compare floating-point (math.h) against CORDIC fixed-point
// results.
      printf("%d/%d: atan=%f (%f), y2 = %d\n", y1, x1, z2/MUL, atan((double)y1/x1), y2);
#endif
    }
  }       
 
  // HYPERBOLIC, VECTORING, SQUARE ROOT
#ifndef DATAGEN
  printf("\nSQUARE ROOT\n");
#endif
  gdirection = VECTORING; gmode = HYPERBOLIC;
  z1 = 0;
  for (w1 = 100; w1 <= 7000; w1+=100)
  {
     x1 = w1+(1<<12);
     y1 = w1-(1<<12);
     cordic(gdirection, gmode, x1, y1, z1, &x2, &y2, &z2);
#ifdef DATAGEN    
// Use this to generate the "cordic_test_data.txt" reference vectors.
    printf("%04x %04x %04x %04x %04x %04x %04x %04x\n", 
      gdirection, gmode, x1&0xffff, y1&0xffff, z1&0xffff, x2&0xffff, y2&0xffff, z2&0xffff);
#else
// Use this to compare floating-point (math.h) against CORDIC fixed-point
// results.
    printf("%d: sqrt=%f (%f)\n", w1, (cordic_1Kp/MUL)*x2/MUL, sqrt((double)w1/MUL));
#endif
  } 
 
  // LINEAR, VECTORING, RECIPROCAL
#ifndef DATAGEN
  printf("\nRECIPROCAL\n");
#endif
  gdirection = VECTORING; gmode = LINEAR;
  z1 = 0;
  y1 = (1<<14);
  for (x1 = 32000; x1 > 8000; x1-=250)
  {
    cordic(gdirection, gmode, x1, y1, z1, &x2, &y2, &z2);
#ifdef DATAGEN    
// Use this to generate the "cordic_test_data.txt" reference vectors.
    printf("%04x %04x %04x %04x %04x %04x %04x %04x\n", 
      gdirection, gmode, x1&0xffff, y1&0xffff, z1&0xffff, x2&0xffff, y2&0xffff, z2&0xffff);
#else
// Use this to compare floating-point (math.h) against CORDIC fixed-point
// results.
    printf("%d: 1/x=%f (%f)\n", x1, z2/MUL, (double)y1/x1);
#endif
  } 
 
  // HYPERBOLIC, VECTORING, SQUARE ROOT + LINEAR, VECTORING, RECIPROCAL
#ifndef DATAGEN
  printf("\nINVERSE SQUARE ROOT\n");
#endif
  for (w1 = 4000; w1 <= 28600; w1+=200)
  {
    gdirection = VECTORING; gmode = HYPERBOLIC; 
    z1 = 0;
    x1 = w1+(1<<12);
    y1 = w1-(1<<12);
    cordic(gdirection, gmode, x1, y1, z1, &x2, &y2, &z2);
#ifdef DATAGEN    
// Use this to generate the "cordic_test_data.txt" reference vectors.
    printf("%04x %04x %04x %04x %04x %04x %04x %04x\n", 
      gdirection, gmode, x1&0xffff, y1&0xffff, z1&0xffff, x2&0xffff, y2&0xffff, z2&0xffff);
#else
// Use this to compare floating-point (math.h) against CORDIC fixed-point
// results.
    printf("%d: sqrt=%f (%f)\n", w1, (cordic_1Kp/MUL)*x2/MUL, sqrt((double)w1/MUL));
#endif
    gdirection = VECTORING; gmode = LINEAR; 
    z1 = 0; 
    y1 = (1<<14); 
    x1 = (cordic_1Kp/MUL)*x2;
    cordic(gdirection, gmode, x1, y1, z1, &x2, &y2, &z2);
#ifdef DATAGEN    
// Use this to generate the "cordic_test_data.txt" reference vectors.
    printf("%04x %04x %04x %04x %04x %04x %04x %04x\n", 
      gdirection, gmode, x1&0xffff, y1&0xffff, z1&0xffff, x2&0xffff, y2&0xffff, z2&0xffff);
#else
// Use this to compare floating-point (math.h) against CORDIC fixed-point
// results.
    printf("%d: 1/sqrt(x)=%f (%f)\n", w1, z2/MUL, sqrt((double)MUL/w1));
#endif
  } 
  return (0);
}