CMS 3D CMS Logo

CordicXilinx.cc
Go to the documentation of this file.
1 
26 
27 #include <vector>
28 #include <iostream>
29 #include <iomanip>
30 #include <cassert>
31 #include <cmath>
32 #include <cmath>
33 
34 CordicXilinx::CordicXilinx(int inputBits, int outputBits, bool debug) :
35  inputBits_(inputBits),
36  outputBits_(outputBits),
37  debug_(debug)
38 {
39  // Coarse rotation lowers necessary iterations by 2
40  iterations_ = outputBits-2;
41  // Internal precision is by default this value (when set to 0 in xilinx config)
42  internalBits_ = outputBits+ceil(log((float) iterations_)/log(2.));
43 
44  double scaleFactor = 1.;
45  for(int i=1; i<=iterations_; ++i)
46  {
47  int rotation = encodeAngle(atan(pow(2.,-i)));
48  rotations_.push_back(rotation);
49  scaleFactor *= pow(1+pow(2.,-2*i), -0.5);
50  }
51  scaleFactor_ = scaleFactor*pow(2., internalBits_-1)+0.5;
52 
53  // Precompute angles table for speed
57 
58  if ( debug_ ) printf("Cordic setup: %d iterations, %d internal bits, scale factor = %d\n", iterations_, internalBits_, scaleFactor_);
59 }
60 
61 int CordicXilinx::encodeAngle(const double angleFloat) const
62 {
63  assert(fabs(angleFloat)<=M_PI);
64  // Xilinx seems to store rounded rotation table
65  return angleFloat*pow(2., internalBits_-3)+0.5;
66 }
67 
68 void CordicXilinx::operator() ( int32_t xInput , int32_t yInput , int32_t& aPhi , uint32_t& aMagnitude ) const
69 {
70  // Assumption in algorithm is that arithmetic shifts are used for ints (as opposed to logical shifts)
71  static_assert( ((int) -1)>>3 == (int) -1 , "Signed ints need to use arithmetic shifts for this algorithm to work properly!");
72 
73  // Input checks
74  // Input is in 2QN format, and for xilinx
75  // the max is +- 1.0000...
76  assert(abs(xInput) <= (1<<(inputBits_-1)));
77  assert(abs(yInput) <= (1<<(inputBits_-1)));
78 
79  // Rotation to get from current vector to origin
80  // must invert to get aPhi
81  int rotation(0);
82  int x,y;
83 
84  // Debug tool
85  auto printVals = [&x,&y,&rotation,this]
86  {
87  printf("x: % 8d y: % 8d phi: % 8d outphi: % 8d float phi = % f\n",
88  x,
89  y,
90  rotation,
91  (abs(rotation)>>(internalBits_-outputBits_)) * ((rotation>0) ? -1:1),
92  rotation/pow(2., internalBits_-3)
93  );
94  };
95 
96  // Convert to internal precision
97  if ( internalBits_ > inputBits_ )
98  {
99  x = xInput << (internalBits_-inputBits_);
100  y = yInput << (internalBits_-inputBits_);
101  }
102  else
103  {
104  x = xInput >> (inputBits_-internalBits_);
105  y = yInput >> (inputBits_-internalBits_);
106  }
107  if ( debug_ ) printVals();
108 
109  // Coarse rotate to [-pi/4,pi/4)
110  if ( x-y >= 0 )
111  {
112  if ( x+y >= 0 )
113  {
114  // East (Correct) quadrant
115  }
116  else
117  {
118  // South, rotate by +pi/2
119  int xtmp = -y;
120  int ytmp = x;
121  x = xtmp;
122  y = ytmp;
123  rotation += encodedAngles_[HalfPi];
124  }
125  }
126  else
127  {
128  if ( x+y >= 0 )
129  {
130  // North, rotate by -pi/2
131  int xtmp = y;
132  int ytmp = -x;
133  x = xtmp;
134  y = ytmp;
135  rotation += encodedAngles_[NHalfPi];
136  }
137  else
138  {
139  // West, rotate by pi
140  x = -x;
141  y = -y;
142  rotation += encodedAngles_[Pi];
143  }
144  }
145  if ( debug_ ) std::cout << "Coarse rotate" << std::endl;
146  if ( debug_ ) printVals();
147 
148  if ( debug_ ) std::cout << "Starting iterations" << std::endl;
149  for ( int i=1; i<=iterations_; ++i )
150  {
151  int sign = (y>=0) ? -1:1;
152  int xtmp = x - sign*(y>>i);
153  int ytmp = y + sign*(x>>i);
154  x = xtmp;
155  y = ytmp;
156  rotation += sign*rotations_[i-1];
157  if ( debug_ ) printVals();
158  }
159 
160  // need a little extra room for the last multiplication
161  aMagnitude = ((long) x * (long) scaleFactor_)>>(2*internalBits_-outputBits_-1);
162 
163  // Xilinx seems to just mod to [-pi,pi]
164  if ( rotation > encodedAngles_[Pi] ) rotation -= 2*encodedAngles_[Pi]+1;
165  aPhi = (-rotation)>>(internalBits_-outputBits_);
166 }
std::vector< int > rotations_
Definition: CordicXilinx.h:27
const int inputBits_
Definition: CordicXilinx.h:23
Abs< T >::type abs(const T &t)
Definition: Abs.h:22
const int outputBits_
Definition: CordicXilinx.h:24
CordicXilinx(int inputBits, int outputBits, bool debug=false)
Definition: CordicXilinx.cc:34
#define M_PI
const bool debug_
Definition: CordicXilinx.h:25
#define debug
Definition: HDRShower.cc:19
void operator()(int32_t xInput, int32_t yInput, int32_t &aPhi, uint32_t &aMagnitude) const
Definition: CordicXilinx.cc:68
int encodeAngle(const double angleFloat) const
Definition: CordicXilinx.cc:61
std::array< int, 3 > encodedAngles_
Definition: CordicXilinx.h:29
Power< A, B >::type pow(const A &a, const B &b)
Definition: Power.h:40