cordic

`timescale 1 ns / 1 ps
// NOTE: 1. Input angle is a modulo of 2*PI scaled to fit in a 32bit register. The user must translate
//          this angle to a value from 0 - (2^32-1).  0 deg = 32'h0, 359.9999... = 32'hFF_FF_FF_FF
//          To translate from degrees to a 32 bit value, multiply 2^32 by the angle (in degrees),
//          then divide by 360
//       2. Size of Xout, Yout is 1 bit larger due to a system gain of 1.647 (which is < 2)
module cordic(clk, zi, xi, yi, zo, xo, yo);
  parameter XY_WIDTH = 16;   // width of input and output data
  parameter Z_WIDTH = 30;   // width of input and output data
  parameter MODE = "ROTOR";
  localparam STG = XY_WIDTH; // same as bit width of X and Y

  input clk;
  // angle is a signed value in the range of -PI to +PI that must be represented as a Z_WIDTH bit signed number
  input  signed [(Z_WIDTH-1):0] zi;
  output signed [(Z_WIDTH-1):0] zo;
  input  signed [(XY_WIDTH-1):0] xi;
  input  signed [(XY_WIDTH-1):0] yi;
  output signed [XY_WIDTH:0] xo;
  output signed [XY_WIDTH:0] yo;
  //------------------------------------------------------------------------------
  //                             arctan table
  //------------------------------------------------------------------------------
  // Note: The atan_table was chosen to be 31 bits wide giving resolution up to atan(2^-30)
  wire signed [31:0] atan [0:30];
  // upper 2 bits = 2'b00 which represents 0 - PI/2 range
  // upper 2 bits = 2'b01 which represents PI/2 to PI range
  // upper 2 bits = 2'b10 which represents PI to 3*PI/2 range (i.e. -PI/2 to -PI)
  // upper 2 bits = 2'b11 which represents 3*PI/2 to 2*PI range (i.e. 0 to -PI/2)
  // The upper 2 bits therefore tell us which quadrant we are in.
  assign atan[00] = 32'b00100000000000000000000000000000; // 45.000 degrees -> atan(2^0)
  assign atan[01] = 32'b00010010111001000000010100011101; // 26.565 degrees -> atan(2^-1)
  assign atan[02] = 32'b00001001111110110011100001011011; // 14.036 degrees -> atan(2^-2)
  assign atan[03] = 32'b00000101000100010001000111010100; // atan(2^-3)
  assign atan[04] = 32'b00000010100010110000110101000011;
  assign atan[05] = 32'b00000001010001011101011111100001;
  assign atan[06] = 32'b00000000101000101111011000011110;
  assign atan[07] = 32'b00000000010100010111110001010101;
  assign atan[08] = 32'b00000000001010001011111001010011;
  assign atan[09] = 32'b00000000000101000101111100101110;
  assign atan[10] = 32'b00000000000010100010111110011000;
  assign atan[11] = 32'b00000000000001010001011111001100;
  assign atan[12] = 32'b00000000000000101000101111100110;
  assign atan[13] = 32'b00000000000000010100010111110011;
  assign atan[14] = 32'b00000000000000001010001011111001;
  assign atan[15] = 32'b00000000000000000101000101111101;
  assign atan[16] = 32'b00000000000000000010100010111110;
  assign atan[17] = 32'b00000000000000000001010001011111;
  assign atan[18] = 32'b00000000000000000000101000101111;
  assign atan[19] = 32'b00000000000000000000010100011000;
  assign atan[20] = 32'b00000000000000000000001010001100;
  assign atan[21] = 32'b00000000000000000000000101000110;
  assign atan[22] = 32'b00000000000000000000000010100011;
  assign atan[23] = 32'b00000000000000000000000001010001;
  assign atan[24] = 32'b00000000000000000000000000101000;
  assign atan[25] = 32'b00000000000000000000000000010100;
  assign atan[26] = 32'b00000000000000000000000000001010;
  assign atan[27] = 32'b00000000000000000000000000000101;
  assign atan[28] = 32'b00000000000000000000000000000010;
  assign atan[29] = 32'b00000000000000000000000000000001; // atan(2^-29)
  assign atan[30] = 32'b00000000000000000000000000000000;
  //stage outputs
  reg signed [XY_WIDTH:0] x [0:(STG-1)];
  reg signed [XY_WIDTH:0] y [0:(STG-1)];
  reg signed [(Z_WIDTH-1):0] z [0:(STG-1)]; // 32bit
  //------------------------------------------------------------------------------
  //                               stage 0
  //------------------------------------------------------------------------------
  generate
  always @(posedge clk)
  begin // make sure the rotation angle is in the -pi/2 to pi/2 range.  If not then pre-rotate
    if(MODE == "ROTOR") begin
      x[0] <= (zi[(Z_WIDTH-1)] == zi[(Z_WIDTH-2)]) ? xi : -xi;
      y[0] <= (zi[(Z_WIDTH-1)] == zi[(Z_WIDTH-2)]) ? yi : -yi;
      z[0] <= { zi[(Z_WIDTH-2)], zi[(Z_WIDTH-2):0] };
    end
    else if (MODE == "VECTOR") begin
      x[0] <= (xi < 0) ? -xi : xi;
      y[0] <= (xi < 0) ? -yi : yi;
      z[0] <= (xi < 0) ?  zi + { 1'b1, {(Z_WIDTH-1){1'b0}}} : zi;
    end
  end
  endgenerate
//------------------------------------------------------------------------------
//                           generate stages 1 to STG-1
//------------------------------------------------------------------------------
  genvar i;
  generate
  for(i = 0; i < (STG-1); i = (i+1))
  begin: XYZ
    wire signed [XY_WIDTH:0] xs = x[i] >>> i; // signed shift right
    wire signed [XY_WIDTH:0] ys = y[i] >>> i;
    //rounded arctan
    wire [(Z_WIDTH-1):0] zs = (Z_WIDTH == 32) ? atan[i] : (Z_WIDTH)'(atan[i] >>> (32-Z_WIDTH)) + 1'(atan[i] >>> (31-Z_WIDTH));
    //the sign of the current rotation angle
    wire s = (MODE == "ROTOR") ? z[i][(Z_WIDTH-1)] : ~y[i][(XY_WIDTH-1)];
    always @(posedge clk)
    begin
    // add/subtract shifted data
      x[(i+1)] <= s ? x[i] + ys : x[i] - ys;
      if (i < STG-2 || MODE == "ROTOR") begin
        y[(i+1)] <= s ? y[i] - xs : y[i] + xs;
      end
      if (i < STG-2 || MODE == "VECTOR") begin
        z[(i+1)] <= s ? z[i] + zs : z[i] - zs;
      end
    end
  end
  endgenerate
  assign xo = x[(STG-1)];
  assign yo = y[(STG-1)];
  assign zo = z[(STG-1)];
endmodule