PandA-2024.02
cordic_atan2.cpp
Go to the documentation of this file.
1 /************************************************
2 Copyright (c) 2016, Xilinx, Inc.
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 1. Redistributions of source code must retain the above copyright notice,
9 this list of conditions and the following disclaimer.
10 
11 2. Redistributions in binary form must reproduce the above copyright notice,
12 this list of conditions and the following disclaimer in the documentation
13 and/or other materials provided with the distribution.
14 
15 3. Neither the name of the copyright holder nor the names of its contributors
16 may be used to endorse or promote products derived from this software
17 without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
21 THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
24 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
27 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ************************************************/
29 
30 #include "cordic.h"
31 /*
32  Gain=1.647;
33  mode 1: rotation; Rotates (x0,y0) by z0
34  (xn,yn)=(cos(z0),sin(z0)) for (x0,y0)=(1,0)
35  x[i+1] = x[i] - y[i] * d[i] * 2^(-1)
36  y[i+1] = y[i] + x[i] * d[i] * 2^(-i)
37  z[i+1] = z[i] - d[i] * atan(2^(-i))
38  mode 0: vectoring; Rotates (x0,y0) to X-axis
39  (xn,zn)=(r,theta)
40  -PI/2<z0<PI/2
41  */
42 
43 //void cordic( bool mode, coord_t x0, coord_t y0, phase_t z0, coord_t *xn, coord_t *yn, phase_t *zn)
44 void cordic_atan2(coord_t y0, coord_t x0, phase_t *zn)
45 
46 {
47  coord_t x, y, xp, yp;
48  phase_t z, zp;
49  bool xneg, yneg, dneg;
50  uint6_t i;
51 
52  const phase_t cord_M_PI = 3.14159265358979323846;
53  const phase_t cord_M_PI_2 = 1.57079632679489661923;
54  // phase_t cord_M_PI_4 = 0.785398163397448309616;
55 
56  static const phase_t atan_2Mi[] = { 0.7854, 0.4636, 0.2450, 0.1244, 0.0624,
57  0.0312, 0.0156, 0.0078, 0.0039 };
58 
59 #ifndef BIT_ACCURATE
60  static const coord_t lut_pow2[] = {1, 2, 4, 8, 16, 32, 64, 128, 256};
61 #endif
62 
63  if (x0 < 0)
64  x = -x0;
65  else
66  x = x0;
67  if (y0 < 0)
68  y = -y0;
69  else
70  y = y0;
71  z = 0;
72 
73  if (y0 == 0) // SPECIAL CASE Y==0
74  {
75  if (x0 < 0)
76  *zn = cord_M_PI;
77  else
78  *zn = 0;
79 
80  return;
81  }
82 
83  if (x0 == 0) // SPECIAL CASE X==0
84  {
85  if (y0 < 0)
86  *zn = -cord_M_PI_2;
87  else
88  *zn = cord_M_PI_2;
89  return;
90  }
91 
92  unsigned char quadrant = 0;
93  if ((x0 > 0) & (y0 > 0))
94  quadrant = 1;
95  else if ((x0 < 0) & (y0 > 0))
96  quadrant = 2;
97  else if ((x0 < 0) & (y0 < 0))
98  quadrant = 3;
99  else if ((x0 > 0) & (y0 < 0))
100  quadrant = 4;
101 
102  LOOP1: for (i = 0; i <= ROT; i++)
103  {
104 
105  #pragma HLS PIPELINE II=1
106 
107  //dneg= (mode==1) ? (z<0) : (y>0);
108  dneg = (y > 0);
109  if (dneg) {
110 #ifdef BIT_ACCURATE
111  xp = x + (y >> i);
112  yp = y - (x >> i);
113 #else
114  xp = x + y/lut_pow2[i]; //x+(y>>i);
115  yp = y - x/lut_pow2[i];//y-(x>>i);
116 #endif
117  zp = z + atan_2Mi[i];
118  } else {
119 #ifdef BIT_ACCURATE
120  xp = x - (y >> i);
121  yp = y + (x >> i);
122 #else
123  xp = x-y/lut_pow2[i]; //x - (y>>i);
124  yp = y+x/lut_pow2[i];//y + (x>>i);
125 #endif
126  zp = z - atan_2Mi[i];
127  }
128  x = xp;
129  y = yp;
130  z = zp;
131  }
132 
133  if (quadrant == 1) {
134  //*xn=x;
135  //*yn=y;
136  *zn = z;
137  } else if (quadrant == 2) {
138  //*xn=x;
139  //*yn=-y;
140  *zn = cord_M_PI - z;
141  } else if (quadrant == 3) {
142  //*xn=-x;
143  //*yn=-y;
144  *zn = z - cord_M_PI;
145  } else if (quadrant == 4) {
146  //*xn=-x;
147  //*yn=-y;
148  *zn = -z;
149  }
150 }
151 
152 void top_atan2(coord_t y0, coord_t x0, phase_t *zn)
153 {
154 #if defined(DB_DOUBLE_PRECISION)
155  *zn = atan2( (double) y0, (double) x0);
156 #elif defined(DB_SINGLE_PRECISION)
157  *zn = atan2f( (float) y0, (float) x0);
158 #elif defined(DB_CORDIC)
159  cordic_atan2(y0, x0, zn);
160 #else
161 #error <UNDEFINED ATAN2 METHOD!>
162 #endif
163 }
#define ROT
Definition: cordic.h:52
void cordic_atan2(coord_t y0, coord_t x0, phase_t *zn)
x
Return the smallest n such that 2^n >= _x.
void top_atan2(coord_t y0, coord_t x0, phase_t *zn)

Generated on Mon Feb 12 2024 13:02:49 for PandA-2024.02 by doxygen 1.8.13