//用CORDIC實現的反正切
 

//      用CORDIC算法實現的2參數反正切。結果的精度與CORDIC的迭代次數有關，迭代次數越多，精度越高。本例子中精確到小數點后4位。要提高迭代次數，還得把增加1QN格式的位數，比如32位long，程序多處需要修改，有需要的話自己改吧。
//      測試時間：10M晶振，51單片機(西門子C515C)，庫函數里提供的actan2函數1.3~6.9ms,x,y越相近時間越短，各個運算時間因參數不同而不同。cordic_myactan函數2.9ms,不因參數不同而變化。
 //     偶爾一次看到DSP實現乘法的資料，才實現了本函數，定義QN為C的整型，其加減乘除和整型一樣。希望對想在單片機里實現CORDIC的朋友有幫助。如有建議或提供更好的實現代碼，感激不盡………… xdkui@sina.com.cn。本人也搜集了些CORDIC的資料，需要的話可以找我。
//代碼如下：
typedef  short int QN1;
typedef short  int QN2 ;
//1QN 2QN數據格式均16位，1QN第一位位符號位，第二位后小數點,定點數。即0x2000=0.5,0xe000=-0.5
//2QN第一位符號位，第3位后小數點，定點數。即0x2000=1,0x1000=0.5
//1QN和2QN是XILINX CORDIC v2.0說明文檔里提到的表示方法，見cordic.pdf
//1QN即相當于定點數里的Q14表示法，2QN相當于Q13,參見《DSP芯片的原理與開發應用(第3版)》(電子工業出版社
//張雄偉等著)第3章 DSP芯片的定點運算
QN2 cordic_myactan(QN1 x, QN1 y);//輸入參數為1QN格式，輸出結果為2QN格式
#define CORDIC_LENGTH 14 //迭代次數
/*#define PI       3.141592653589793
#define HALF_PI  (PI * 0.5)
double CORDIC_TABLE[CORDIC_LENGTH]={0.785398,0.463648,0.244979,0.124355,0.062419,0.031240,
0.015624,0.007812,0.003906,0.001953,0.000977,0.000488,0.000244,0.000122,0.000061,0.000031,
0.000015,0.000008,0.000004,0.000002,0.000001};*/
//上面的浮點數對應的2QN格式的定點數為
#define PI       0x6487
#define HALF_PI  0x3243
QN2 CORDIC_TABLE[CORDIC_LENGTH]={0x1921,0xED6,0x7D6,0x3FA,0x1FF,0xFF,0x7F,0x3F,0x1F,0xF,
0x8,0x3,0x1,0x0/*,0x0,0x0,0x0,0x0,0x0,0x0,0x0*/};

QN2 cordic_myactan(QN1 x, QN1 y)
{
  int L;
  QN1 tmp_I;
  QN2   phase_rads, acc_phase_rads;
  
  if (x < 0) {
    /* rotate by an initial +/- 90 degrees */
    tmp_I = x;
    if (y > 0) {
       x = y;  /* subtract 90 degrees */
       y = -tmp_I;
       acc_phase_rads = -HALF_PI;
    } else {
       x = -y;  /* add 90 degrees */
       y = tmp_I;
       acc_phase_rads = HALF_PI;
    }
  } else {
    acc_phase_rads = 0;
  }
  
  
  /* rotate using "1 + jK" factors */
  for (L = 0; L <= CORDIC_LENGTH; L++) {
   // K = mp_cordic_table[L].K;
    phase_rads = CORDIC_TABLE[L];
    tmp_I = x;
    if (y >= 0) {
      /* phase is positive: do negative roation */
      x += y>>L;//*pow(2,-L); // K  K=2的-L次方,見CORDIC例子代碼;
      y -= tmp_I>>L;// *pow(2,-L);
      acc_phase_rads -= phase_rads;
    } else {
      /* phase is negative: do positive rotation */
      x -= y >>L;//*pow(2,-L);
      y += tmp_I>>L;//*pow(2,-L);
      acc_phase_rads += phase_rads;
    }
  }
  return -acc_phase_rads;
}