评分

精Cordic算法FPGA实现cos,sin (含python和verilog代码)

chipdebug

1年前更新

12803

该帖子部分内容已隐藏

“ Cordic算法是一种特别适合于数字硬件电路实现各种三角函数与反三角函数以及计算直角三角形第三边长的算法。”

01 Cordic算法原理与python代码实现

如上图所示，当点（x1,y1）转过θ角之后，到达点（x2,y2），有如下等式关系（根据三角函数的角度加减运算推导一下就出来了）。

x2 = rcos(θ0+θ) = x1cosθ-y1sinθ = cosθ(x1 - y1tanθ)
y2 = rsin(θ0+θ) = x1sinθ+y1cosθ = cosθ(y1 + x1tanθ)

Codic算法思想就是循环迭代旋转，逐渐向所求结果逼近，每次迭代旋转的角度都减半，旋转之后如果大于所要求的角度值，就顺时钟回转，否则逆时钟转动，如下图动画所示（演示一个向70度角逼近的过程）。

接下来，确定一下每次旋转的角度。在这里我们为了便于硬件额实现，做以下考虑。
旋转n次后得到的值如下：

xn=cosθ0cosθ1cosθ2...cosθn(xn-1 - yn-1tanθn)
yn=cosθ0cosθ1cosθ2...cosθn(yn-1 + xn-1tanθn)
令 k = cosθ0cosθ1cosθ2...cosθn
当旋转次数与每次旋转的角度值确定后，k值就确定了，可以把它当作一个固定的系数。
但是每次运算还要做两个乘法运算：xn - yntanθn ， yn + xntanθn 。
因此考虑将每次旋转的角度值θn都定为使得tanθn = (1/2)^n,这样乘法运算变成只需要做移位操作就可以了。

首先我们使用python来得到k和每次旋转的角度，方案定为旋转16次。代码如下：

import numpy as np
import math
#get the arctan of 1,1/2,1/4....
arctan_value = np.zeros((16,))
for i in range(0,16) :
    arctan_value[i] = (1/2)**i
print(arctan_value)
angle_e = np.degrees(np.arctan(arctan_value))
print(angle_e)
f = open('cordic_arctan.v','w')
f.write( "\n get the arctan of 1,1/2,1/4...." )
for i in range(0,16):
    f.write("\n `define angle"+ str(i) +" " + str(angle_e[i]))
f.write( "\n\n get the cos_e of 16 iterations" )   
cos_e = np.cos(angle_e*np.pi/180)
k = 1
for i in range(0,16):
    k = cos_e[i]*k
    f.write( "\n cos_"+ str(i) + ":  " + str(cos_e[i]) )
f.write("\n\n get K = cos_0*cos_1*cos_2... = "+str(k))  
f.close()

得到（angle为每次旋转的角度）：

`define angle0  45.0                             
`define angle1  26.56505117707799                
`define angle2  14.036243467926479               
`define angle3  7.125016348901798                
`define angle4  3.576334374997351                
`define angle5  1.789910608246069               
`define angle6  0.895173710211074               
`define angle7  0.44761417086055               
`define angle8  0.22381050036853          
`define angle9  0.11190567706620       
`define angle10 0.05595289189380      
`define angle11 0.02797645261700        
`define angle12 0.01398822714226            
`define angle13 0.00699411367535          
`define angle14 0.00349705685070           
`define angle15 0.00174852842698

get the cos_e of 16 iterations
cos_0: 0.7071067811865476
cos_1: 0.8944271909999159
cos_2: 0.9701425001453319
cos_3: 0.9922778767136676
cos_4: 0.9980525784828885
cos_5: 0.9995120760870788
cos_6: 0.9998779520346953
cos_7: 0.9999694838187878
cos_8: 0.9999923706927791
cos_9: 0.9999980926568242
cos_10: 0.9999995231631829
cos_11: 0.9999998807907318
cos_12: 0.999999970197679
cos_13: 0.9999999925494195
cos_14: 0.9999999981373549
cos_15: 0.9999999995343387

get K = cos_0*cos_1*cos_2... = 0.6072529351031395

其Cordic计算cos,sin值的python代码实现如下（angle_e[i]里面存储了每次旋转的角度值）：

x = 1                                      
y = 0                                      
angle = 70  #需要求得角度                  
x_p = np.zeros((16,2))                     
y_p = np.zeros((16,2))                     

for i in range(0,16):                      
    if angle > 0:                          
        x_h = x - y*arctan_value[i]        
        y_h = y + x*arctan_value[i]        
        angle = angle - angle_e[i]         
    else :                                 
        x_h = x + y*arctan_value[i]        
        y_h = y - x*arctan_value[i]        
        angle = angle + angle_e[i]         
    x = x_h               
    y = y_h                                         

cos_angle = x*k      #所求角度的cos值                        
sin_angle = y*k       #所求角度的sin值

注意：这里的cordic可以计算的角度角度的范围为
（-angle[0]+angle[1]+…+angle[15] ，+angle[0]+angle[1]+…+angle[15]）。
上例求得是角度为70度的角，十六次迭代过程如下视屏所示。

02 verilog实现

为了便于硬件实现少一个乘法器运算，把上面的软件改成：

x = k        #初始化为K，因此避免最后需要乘以k                              
y = 0                                      
angle = 70  #需要求得角度                  
x_p = np.zeros((16,2))                     
y_p = np.zeros((16,2))                     

for i in range(0,16):                      
    if angle > 0:                          
        x_h = x - y*arctan_value[i]        
        y_h = y + x*arctan_value[i]        
        angle = angle - angle_e[i]         
    else :                                 
        x_h = x + y*arctan_value[i]        
        y_h = y - x*arctan_value[i]        
        angle = angle + angle_e[i]         
    x = x_h               
    y = y_h                                         

cos_angle = x      #所求角度的cos值                        
sin_angle = y      #所求角度的sin值

硬件电路采用16级流水线设计，（如果对流水线不清楚，可以参考文章 “一个实例彻底拿下流水线设计”），同时为了避免浮点数运算，将数据整体扩大了65535倍，即向左移16位.。所得的cos与sin的真实结果为电路输出值除以65535。下篇文章将会讲述Cordic算法实现 distance=√(x^2+y^2 )，即根据两直角边求斜边长。

此处内容已隐藏，请付费后查看

FPGA

评分

欢迎为Ta评分

只看作者

最新最热

- BertramChen0
  有代码，有实例，楼主威武！
  5年前回复
- ipqsn0
  这个还是蛮实用的，在电机控制等方面应用还是蛮广的，点一个赞！
  5年前回复

chipdebug

作者超级版主0

Cordic_cos.v


module Cordic_cos
(
    CLK_50M,RST_N,
    angle,
    start,
    finished,
    Sin,
    Cos
);

input                           CLK_50M;
input                           RST_N;
input         [8:0]     angle;
input               start;

output              finished;
output        [31:0]         Sin;
output        [31:0]         Cos;

`define angle_0  32'd2949120        //45°*2^16
`define angle_1  32'd1740992        //26.5651°*2^16
`define angle_2  32'd919872        //14.0362°*2^16
`define angle_3  32'd466944        //7.1250°*2^16
`define angle_4  32'd234368        //3.5763°*2^16
`define angle_5  32'd117312        //1.7899°*2^16
`define angle_6  32'd58688        //0.8952°*2^16
`define angle_7  32'd29312        //0.4476°*2^16
`define angle_8  32'd14656        //0.2238°*2^16
`define angle_9  32'd7360            //0.1119°*2^16
`define angle_10 32'd3648            //0.0560°*2^16
`define angle_11 32'd1856            //0.0280°*2^16
`define angle_12 32'd896            //0.0140°*2^16
`define angle_13 32'd448            //0.0070°*2^16
`define angle_14 32'd256            //0.0035°*2^16
`define angle_15 32'd128            //0.0018°*2^16

parameter Pipeline = 16;
parameter K = 32'h09b74;    //K=0.607253*2^16,32'h09b74,

reg signed     [31:0]         Sin;
reg signed     [31:0]         Cos;
reg signed     [31:0]         x0=0,y0=0,z0=0;
reg signed     [31:0]         x1=0,y1=0,z1=0;
reg signed     [31:0]         x2=0,y2=0,z2=0;
reg signed     [31:0]         x3=0,y3=0,z3=0;
reg signed     [31:0]         x4=0,y4=0,z4=0;
reg signed     [31:0]         x5=0,y5=0,z5=0;
reg signed     [31:0]         x6=0,y6=0,z6=0;
reg signed     [31:0]         x7=0,y7=0,z7=0;
reg signed     [31:0]         x8=0,y8=0,z8=0;
reg signed     [31:0]         x9=0,y9=0,z9=0;
reg signed     [31:0]         x10=0,y10=0,z10=0;
reg signed     [31:0]         x11=0,y11=0,z11=0;
reg signed     [31:0]         x12=0,y12=0,z12=0;
reg signed     [31:0]         x13=0,y13=0,z13=0;
reg signed     [31:0]         x14=0,y14=0,z14=0;
reg signed     [31:0]         x15=0,y15=0,z15=0;
reg signed     [31:0]         x16=0,y16=0,z16=0;

reg  [4:0]           count;

always@ (posedge CLK_50M or negedge RST_N) begin
    if(!RST_N)
      count <= 4'b00;
    else if( start ) begin
        if( count!=5'd18 )
        count <= count+1'b1;
      else if( count == 5'd18 )
        count <= count;
    end
    else
      count <= 5'h00;
end

assign finished = (count == 5'd18)?1'b1:1'b0;

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x0 <= 1'b0;                         
        y0 <= 1'b0;
        z0 <= 1'b0;
    end
    else
    begin
        x0 <= K;
        y0 <= 32'd0;
        z0 <= angle << 16;
    end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x1 <= 1'b0;                         
        y1 <= 1'b0;
        z1 <= 1'b0;
    end
    else if(z0[31])
    begin
      x1 <= x0 + y0;
      y1 <= y0 - x0;
      z1 <= z0 + `angle_0;
    end
    else
    begin
      x1 <= x0 - y0;
      y1 <= y0 + x0;
      z1 <= z0 - `angle_0;
    end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x2 <= 1'b0;                         
        y2 <= 1'b0;
        z2 <= 1'b0;
    end
    else if(z1[31])
   begin
        x2 <= x1 + (y1 >>> 1);
        y2 <= y1 - (x1 >>> 1);
        z2 <= z1 + `angle_1;
   end
   else
   begin
       x2 <= x1 - (y1 >>> 1);
       y2 <= y1 + (x1 >>> 1);
       z2 <= z1 - `angle_1;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x3 <= 1'b0;                         
        y3 <= 1'b0;
        z3 <= 1'b0;
    end
    else if(z2[31])
   begin
       x3 <= x2 + (y2 >>> 2);
       y3 <= y2 - (x2 >>> 2);
       z3 <= z2 + `angle_2;
   end
   else
   begin
       x3 <= x2 - (y2 >>> 2);
       y3 <= y2 + (x2 >>> 2);
       z3 <= z2 - `angle_2;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x4 <= 1'b0;                         
        y4 <= 1'b0;
        z4 <= 1'b0;
    end
    else if(z3[31])
   begin
       x4 <= x3 + (y3 >>> 3);
       y4 <= y3 - (x3 >>> 3);
       z4 <= z3 + `angle_3;
   end
   else
   begin
       x4 <= x3 - (y3 >>> 3);
       y4 <= y3 + (x3 >>> 3);
       z4 <= z3 - `angle_3;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x5 <= 1'b0;                         
        y5 <= 1'b0;
        z5 <= 1'b0;
    end
    else if(z4[31])
   begin
       x5 <= x4 + (y4 >>> 4);
       y5 <= y4 - (x4 >>> 4);
       z5 <= z4 + `angle_4;
   end
   else
   begin
       x5 <= x4 - (y4 >>> 4);
       y5 <= y4 + (x4 >>> 4);
       z5 <= z4 - `angle_4;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x6 <= 1'b0;                         
        y6 <= 1'b0;
        z6 <= 1'b0;
    end
    else if(z5[31])
   begin
       x6 <= x5 + (y5 >>> 5);
       y6 <= y5 - (x5 >>> 5);
       z6 <= z5 + `angle_5;
   end
   else
   begin
       x6 <= x5 - (y5 >>> 5);
       y6 <= y5 + (x5 >>> 5);
       z6 <= z5 - `angle_5;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x7 <= 1'b0;                         
        y7 <= 1'b0;
        z7 <= 1'b0;
    end
    else if(z6[31])
   begin
       x7 <= x6 + (y6 >>> 6);
       y7 <= y6 - (x6 >>> 6);
       z7 <= z6 + `angle_6;
   end
   else
   begin
       x7 <= x6 - (y6 >>> 6);
       y7 <= y6 + (x6 >>> 6);
       z7 <= z6 - `angle_6;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x8 <= 1'b0;                         
        y8 <= 1'b0;
        z8 <= 1'b0;
    end
    else if(z7[31])
   begin
       x8 <= x7 + (y7 >>> 7);
       y8 <= y7 - (x7 >>> 7);
       z8 <= z7 + `angle_7;
   end
   else
   begin
       x8 <= x7 - (y7 >>> 7);
       y8 <= y7 + (x7 >>> 7);
       z8 <= z7 - `angle_7;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x9 <= 1'b0;                         
        y9 <= 1'b0;
        z9 <= 1'b0;
    end
    else if(z8[31])
   begin
       x9 <= x8 + (y8 >>> 8);
       y9 <= y8 - (x8 >>> 8);
       z9 <= z8 + `angle_8;
   end
   else
   begin
       x9 <= x8 - (y8 >>> 8);
       y9 <= y8 + (x8 >>> 8);
       z9 <= z8 - `angle_8;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x10 <= 1'b0;                         
        y10 <= 1'b0;
        z10 <= 1'b0;
    end
    else if(z9[31])
   begin
       x10 <= x9 + (y9 >>> 9);
       y10 <= y9 - (x9 >>> 9);
       z10 <= z9 + `angle_9;
   end
   else
   begin
       x10 <= x9 - (y9 >>> 9);
       y10 <= y9 + (x9 >>> 9);
       z10 <= z9 - `angle_9;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x11 <= 1'b0;                         
        y11 <= 1'b0;
        z11 <= 1'b0;
    end
    else if(z10[31])
   begin
       x11 <= x10 + (y10 >>> 10);
       y11 <= y10 - (x10 >>> 10);
       z11 <= z10 + `angle_10;
   end
   else
   begin
       x11 <= x10 - (y10 >>> 10);
       y11 <= y10 + (x10 >>> 10);
       z11 <= z10 - `angle_10;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x12 <= 1'b0;                         
        y12 <= 1'b0;
        z12 <= 1'b0;
    end
    else if(z11[31])
   begin
       x12 <= x11 + (y11 >>> 11);
       y12 <= y11 - (x11 >>> 11);
       z12 <= z11 + `angle_11;
   end
   else
   begin
       x12 <= x11 - (y11 >>> 11);
       y12 <= y11 + (x11 >>> 11);
       z12 <= z11 - `angle_11;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x13 <= 1'b0;                         
        y13 <= 1'b0;
        z13 <= 1'b0;
    end
    else if(z12[31])
   begin
       x13 <= x12 + (y12 >>> 12);
       y13 <= y12 - (x12 >>> 12);
       z13 <= z12 + `angle_12;
   end
   else
   begin
       x13 <= x12 - (y12 >>> 12);
       y13 <= y12 + (x12 >>> 12);
       z13 <= z12 - `angle_12;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x14 <= 1'b0;                         
        y14 <= 1'b0;
        z14 <= 1'b0;
    end
    else if(z13[31])
   begin
       x14 <= x13 + (y13 >>> 13);
       y14 <= y13 - (x13 >>> 13);
       z14 <= z13 + `angle_13;
   end
   else
   begin
       x14 <= x13 - (y13 >>> 13);
       y14 <= y13 + (x13 >>> 13);
       z14 <= z13 - `angle_13;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x15 <= 1'b0;                         
        y15 <= 1'b0;
        z15 <= 1'b0;
    end
    else if(z14[31])
   begin
       x15 <= x14 + (y14 >>> 14);
       y15 <= y14 - (x14 >>> 14);
       z15 <= z14 + `angle_14;
   end
   else
   begin
       x15 <= x14 - (y14 >>> 14);
       y15 <= y14 + (x14 >>> 14);
       z15 <= z14 - `angle_14;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N)
    begin
        x16 <= 1'b0;                         
        y16 <= 1'b0;
        z16 <= 1'b0;
    end
    else if(z15[31])
   begin
       x16 <= x15 + (y15 >>> 15);
       y16 <= y15 - (x15 >>> 15);
       z16 <= z15 + `angle_15;
   end
   else
   begin
       x16 <= x15 - (y15 >>> 15);
       y16 <= y15 + (x15 >>> 15);
       z16 <= z15 - `angle_15;
   end
end

always @ (posedge CLK_50M or negedge RST_N)
begin
    if(!RST_N) begin
        Cos <= 1'b0;
        Sin <= 1'b0;
    end
    else begin
        Cos <= x16;
        Sin <= y16;
    end
end

endmodule

Cordic_tb.v

`timescale 1 ps/ 1 ps

module Cordic_Test_tb;

// Inputs
reg                             CLK_50M;
reg                             RST_N;
reg              [15:0]         cnt;
reg              [15:0]         cnt_n;
reg              [8:0]         angle;
reg                            start;
wire              [31:0]         Sin;
wire              [31:0]         Cos;
wire              [31:0]         Error;

// Instantiate the Unit Under Test (UUT)
Cordic_cos                 uut 
(
    .CLK_50M              (CLK_50M        ),
    .RST_N                    (RST_N        ),
    .angle                    (angle        ),
    .start          (start    ),
    .finished       (finished ),
    .Sin                        (Sin            ),
    .Cos                        (Cos            )
);




initial
begin
    #0 CLK_50M = 1'b0;
       start = 1'b0;
       angle = 9'b000;
    #100 RST_N = 1'b0;
    #100 RST_N = 1'b1;

    #100 start = 1'b1;
         angle = 9'd60;
    #1000000 $stop;
end 

always #100
begin
    CLK_50M = ~CLK_50M;
end



endmodule

5年前