一个CORDIC算法在圆周系统下的向量模式下获取角度的Verilog 程序
下面给出一个CORDIC算法在圆周系统下的向量模式下获取角度的Verilog 程序：
/*==============================================================================*\
Filename : Cordic.v
Discription : 坐标旋转数字计算方法。通过该算法，对输入的向量坐标进行9次迭代
计算，得到该向量的模值和相角。
\*==============================================================================*/
Filename : Cordic.v
Discription : 坐标旋转数字计算方法。通过该算法，对输入的向量坐标进行9次迭代
计算，得到该向量的模值和相角。
\*==============================================================================*/
module CORDIC
(
Clk_20m,
_Rst,
Cordic_start,
Ug_d,
Ug_q,
Ug,
Delta
(
Clk_20m,
_Rst,
Cordic_start,
Ug_d,
Ug_q,
Ug,
Delta
);
input Clk_20m,
_Rst,
Cordic_start; //CORDIC变换启动标志
input[15:0] Ug_d, //输出电压的d轴分量
Ug_q; //输出电压的q轴分量
output[15:0] Ug; //输出电压向量的模值
output[13:0] Delta; //输出电压向量的相角
wire[31:0] Ug_tmp;
reg[3:0] Times; //迭代次数累加器
reg[15:0] Ug_d_tmp, //输出电压d轴分量的中间迭代结果
Ug_q_tmp; //输出电压q轴分量的中间迭代结果
reg[13:0] //Delta,
Delta_tmp; //相位角旋转累加寄存器
// assign Ug = ( Ug_d_tmp>>1 ) + ( Ug_d_tmp>>3 ) - ( Ug_d_tmp>>6 ) - ( Ug_d_tmp>>9 );
//对电压模值进行比例系数调整，得到实际模值的32倍
// assign Ug_tmp[31:0] = Ug_d_tmp[15:0] * 16'd48224;//d39797;
assign Ug_tmp[31:0] = Ug_d_tmp[15:0] * 16'd45208;
assign Ug[15:0] = Ug_tmp[31:16];
_Rst,
Cordic_start; //CORDIC变换启动标志
input[15:0] Ug_d, //输出电压的d轴分量
Ug_q; //输出电压的q轴分量
output[15:0] Ug; //输出电压向量的模值
output[13:0] Delta; //输出电压向量的相角
wire[31:0] Ug_tmp;
reg[3:0] Times; //迭代次数累加器
reg[15:0] Ug_d_tmp, //输出电压d轴分量的中间迭代结果
Ug_q_tmp; //输出电压q轴分量的中间迭代结果
reg[13:0] //Delta,
Delta_tmp; //相位角旋转累加寄存器
// assign Ug = ( Ug_d_tmp>>1 ) + ( Ug_d_tmp>>3 ) - ( Ug_d_tmp>>6 ) - ( Ug_d_tmp>>9 );
//对电压模值进行比例系数调整，得到实际模值的32倍
// assign Ug_tmp[31:0] = Ug_d_tmp[15:0] * 16'd48224;//d39797;
assign Ug_tmp[31:0] = Ug_d_tmp[15:0] * 16'd45208;
assign Ug[15:0] = Ug_tmp[31:16];
//输出电压向量的相角即为CORDIC算法输出的旋转角
assign Delta = Delta_tmp;
/*
always @( posedge Clk_20m or negedge _Rst )
begin
if ( !_Rst )
Delta <= 14'h0;
else if ( Delta_tmp <= 14'h6 )
Delta <= Delta_tmp;
else if ( Delta_tmp <= 14'h1fff )
Delta <= 14'h6;
else if ( Delta_tmp <= 14'h3ffa )
Delta <= 14'h3ffa;
else
Delta <= Delta_tmp;
assign Delta = Delta_tmp;
/*
always @( posedge Clk_20m or negedge _Rst )
begin
if ( !_Rst )
Delta <= 14'h0;
else if ( Delta_tmp <= 14'h6 )
Delta <= Delta_tmp;
else if ( Delta_tmp <= 14'h1fff )
Delta <= 14'h6;
else if ( Delta_tmp <= 14'h3ffa )
Delta <= 14'h3ffa;
else
Delta <= Delta_tmp;
else
Delta <= 14'h6;
end
*/
always @( posedge Clk_20m or negedge _Rst )
begin
if ( !_Rst )
begin
Times[3:0] <= 4'hf;
Ug_d_tmp[15:0] <= 16'h0;
Ug_q_tmp[15:0] <= 16'h0;
Delta_tmp[13:0] <= 14'h0;
end
else if ( Cordic_start ) //启动CORDIC变换
begin
Times[3:0] <= 4'h0;
Ug_d_tmp <= Ug_d;
Ug_q_tmp <= Ug_q;
Delta_tmp <= 14'h0;
end
else if ( Times <= 4'd9 ) //开始迭代计算
begin
Times[3:0] <= Times[3:0] + 4'h1; //迭代次数加1
case ( Times )
4'h0:
//Ug_q_tmp[15] 符号位
if ( Ug_q_tmp[15] ) //旋转的目标是使Ug_q_tmp趋近于0,
//根据对Ug_q_tmp符号的判断，决定正向旋转还是反向旋转
begin
Ug_d_tmp <= Ug_d_tmp - Ug_q_tmp; //重新计算新的d轴分量
Ug_q_tmp <= Ug_q_tmp + Ug_d_tmp; //重新计算新的q轴分量
Delta_tmp <= Delta_tmp - 14'hB40; //对相位角进行累加计算
end
else
begin
Ug_d_tmp <= Ug_d_tmp + Ug_q_tmp;
Ug_q_tmp <= Ug_q_tmp - Ug_d_tmp;
Delta_tmp <= Delta_tmp + 14'hB40; //(2880/64)=45
end
4'h1:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { Ug_q_tmp[15], Ug_q_tmp[15:1] };
Ug_q_tmp <= Ug_q_tmp + { Ug_d_tmp[15], Ug_d_tmp[15:1] };
Delta_tmp <= Delta_tmp - 14'h6A4; // (1700/64)=26.5625
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { Ug_q_tmp[15], Ug_q_tmp[15:1] };
Ug_q_tmp <= Ug_q_tmp - { Ug_d_tmp[15], Ug_d_tmp[15:1] };
Delta_tmp <= Delta_tmp + 14'h6A4; //
end
4'h2:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {2{Ug_q_tmp[15]}}, Ug_q_tmp[15:2] };
Ug_q_tmp <= Ug_q_tmp + { {2{Ug_d_tmp[15]}}, Ug_d_tmp[15:2] };
Delta_tmp <= Delta_tmp - 14'h382; // (382/64=14.03125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {2{Ug_q_tmp[15]}}, Ug_q_tmp[15:2] };
Ug_q_tmp <= Ug_q_tmp - { {2{Ug_d_tmp[15]}}, Ug_d_tmp[15:2] };
Delta_tmp <= Delta_tmp + 14'h382;
end
4'h3:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {3{Ug_q_tmp[15]}}, Ug_q_tmp[15:3] };
Ug_q_tmp <= Ug_q_tmp + { {3{Ug_d_tmp[15]}}, Ug_d_tmp[15:3] };
Delta_tmp <= Delta_tmp - 14'h1c8; // (382/64=14.03125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {3{Ug_q_tmp[15]}}, Ug_q_tmp[15:3] };
Ug_q_tmp <= Ug_q_tmp - { {3{Ug_d_tmp[15]}}, Ug_d_tmp[15:3] };
Delta_tmp <= Delta_tmp + 14'h1c8; // (456/64=7.125)
end
4'h4:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {4{Ug_q_tmp[15]}}, Ug_q_tmp[15:4] };
Ug_q_tmp <= Ug_q_tmp + { {4{Ug_d_tmp[15]}}, Ug_d_tmp[15:4] };
Delta_tmp <= Delta_tmp - 14'hE5; //(229/64=3.578125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {4{Ug_q_tmp[15]}}, Ug_q_tmp[15:4] };
Ug_q_tmp <= Ug_q_tmp - { {4{Ug_d_tmp[15]}}, Ug_d_tmp[15:4] };
Delta_tmp <= Delta_tmp + 14'hE5;
end
4'h5:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {5{Ug_q_tmp[15]}}, Ug_q_tmp[15:5] };
Ug_q_tmp <= Ug_q_tmp + { {5{Ug_d_tmp[15]}}, Ug_d_tmp[15:5] };
Delta_tmp <= Delta_tmp - 14'h72; //(114/64=1.78125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {5{Ug_q_tmp[15]}}, Ug_q_tmp[15:5] };
Ug_q_tmp <= Ug_q_tmp - { {5{Ug_d_tmp[15]}}, Ug_d_tmp[15:5] };
Delta_tmp <= Delta_tmp + 14'h72;
end
4'h6:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {6{Ug_q_tmp[15]}}, Ug_q_tmp[15:6] };
Ug_q_tmp <= Ug_q_tmp + { {6{Ug_d_tmp[15]}}, Ug_d_tmp[15:6] };
Delta_tmp <= Delta_tmp - 14'h39;//(57/64=0.890625)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {6{Ug_q_tmp[15]}}, Ug_q_tmp[15:6] };
Ug_q_tmp <= Ug_q_tmp - { {6{Ug_d_tmp[15]}}, Ug_d_tmp[15:6] };
Delta_tmp <= Delta_tmp + 14'h39;
end
4'h7:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {7{Ug_q_tmp[15]}}, Ug_q_tmp[15:7] };
Ug_q_tmp <= Ug_q_tmp + { {7{Ug_d_tmp[15]}}, Ug_d_tmp[15:7] };
Delta_tmp <= Delta_tmp - 14'h1C;//(28/64=0.4375)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {7{Ug_q_tmp[15]}}, Ug_q_tmp[15:7] };
Ug_q_tmp <= Ug_q_tmp - { {7{Ug_d_tmp[15]}}, Ug_d_tmp[15:7] };
Delta_tmp <= Delta_tmp + 14'h1C;
end
4'h8:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {8{Ug_q_tmp[15]}}, Ug_q_tmp[15:8] };
Ug_q_tmp <= Ug_q_tmp + { {8{Ug_d_tmp[15]}}, Ug_d_tmp[15:8] };
Delta_tmp <= Delta_tmp - 14'hE;//(14/64=0.21875)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {8{Ug_q_tmp[15]}}, Ug_q_tmp[15:8] };
Ug_q_tmp <= Ug_q_tmp - { {8{Ug_d_tmp[15]}}, Ug_d_tmp[15:8] };
Delta_tmp <= Delta_tmp + 14'hE;
end
4'h9:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {9{Ug_q_tmp[15]}}, Ug_q_tmp[15:9] };
Ug_q_tmp <= Ug_q_tmp + { {9{Ug_d_tmp[15]}}, Ug_d_tmp[15:9] };
Delta_tmp <= Delta_tmp - 14'h7;
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {9{Ug_q_tmp[15]}}, Ug_q_tmp[15:9] };
Ug_q_tmp <= Ug_q_tmp - { {9{Ug_d_tmp[15]}}, Ug_d_tmp[15:9] };
Delta_tmp <= Delta_tmp + 14'h7; //(7/64=0.109375)
end
default: //缺省情况下所有寄存器清零
begin
Ug_d_tmp <= 16'h0;
Ug_q_tmp <= 16'h0;
Delta_tmp <= 14'h0;
end
// ;
endcase
end
else
Times[3:0] <= 4'hf; //迭代计算完毕，结束CORDIC算法，迭代次数置复位值
end
if ( Ug_q_tmp[15] ) //旋转的目标是使Ug_q_tmp趋近于0,
//根据对Ug_q_tmp符号的判断，决定正向旋转还是反向旋转
begin
Ug_d_tmp <= Ug_d_tmp - Ug_q_tmp; //重新计算新的d轴分量
Ug_q_tmp <= Ug_q_tmp + Ug_d_tmp; //重新计算新的q轴分量
Delta_tmp <= Delta_tmp - 14'hB40; //对相位角进行累加计算
end
else
begin
Ug_d_tmp <= Ug_d_tmp + Ug_q_tmp;
Ug_q_tmp <= Ug_q_tmp - Ug_d_tmp;
Delta_tmp <= Delta_tmp + 14'hB40; //(2880/64)=45
end
4'h1:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { Ug_q_tmp[15], Ug_q_tmp[15:1] };
Ug_q_tmp <= Ug_q_tmp + { Ug_d_tmp[15], Ug_d_tmp[15:1] };
Delta_tmp <= Delta_tmp - 14'h6A4; // (1700/64)=26.5625
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { Ug_q_tmp[15], Ug_q_tmp[15:1] };
Ug_q_tmp <= Ug_q_tmp - { Ug_d_tmp[15], Ug_d_tmp[15:1] };
Delta_tmp <= Delta_tmp + 14'h6A4; //
end
4'h2:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {2{Ug_q_tmp[15]}}, Ug_q_tmp[15:2] };
Ug_q_tmp <= Ug_q_tmp + { {2{Ug_d_tmp[15]}}, Ug_d_tmp[15:2] };
Delta_tmp <= Delta_tmp - 14'h382; // (382/64=14.03125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {2{Ug_q_tmp[15]}}, Ug_q_tmp[15:2] };
Ug_q_tmp <= Ug_q_tmp - { {2{Ug_d_tmp[15]}}, Ug_d_tmp[15:2] };
Delta_tmp <= Delta_tmp + 14'h382;
end
4'h3:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {3{Ug_q_tmp[15]}}, Ug_q_tmp[15:3] };
Ug_q_tmp <= Ug_q_tmp + { {3{Ug_d_tmp[15]}}, Ug_d_tmp[15:3] };
Delta_tmp <= Delta_tmp - 14'h1c8; // (382/64=14.03125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {3{Ug_q_tmp[15]}}, Ug_q_tmp[15:3] };
Ug_q_tmp <= Ug_q_tmp - { {3{Ug_d_tmp[15]}}, Ug_d_tmp[15:3] };
Delta_tmp <= Delta_tmp + 14'h1c8; // (456/64=7.125)
end
4'h4:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {4{Ug_q_tmp[15]}}, Ug_q_tmp[15:4] };
Ug_q_tmp <= Ug_q_tmp + { {4{Ug_d_tmp[15]}}, Ug_d_tmp[15:4] };
Delta_tmp <= Delta_tmp - 14'hE5; //(229/64=3.578125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {4{Ug_q_tmp[15]}}, Ug_q_tmp[15:4] };
Ug_q_tmp <= Ug_q_tmp - { {4{Ug_d_tmp[15]}}, Ug_d_tmp[15:4] };
Delta_tmp <= Delta_tmp + 14'hE5;
end
4'h5:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {5{Ug_q_tmp[15]}}, Ug_q_tmp[15:5] };
Ug_q_tmp <= Ug_q_tmp + { {5{Ug_d_tmp[15]}}, Ug_d_tmp[15:5] };
Delta_tmp <= Delta_tmp - 14'h72; //(114/64=1.78125)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {5{Ug_q_tmp[15]}}, Ug_q_tmp[15:5] };
Ug_q_tmp <= Ug_q_tmp - { {5{Ug_d_tmp[15]}}, Ug_d_tmp[15:5] };
Delta_tmp <= Delta_tmp + 14'h72;
end
4'h6:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {6{Ug_q_tmp[15]}}, Ug_q_tmp[15:6] };
Ug_q_tmp <= Ug_q_tmp + { {6{Ug_d_tmp[15]}}, Ug_d_tmp[15:6] };
Delta_tmp <= Delta_tmp - 14'h39;//(57/64=0.890625)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {6{Ug_q_tmp[15]}}, Ug_q_tmp[15:6] };
Ug_q_tmp <= Ug_q_tmp - { {6{Ug_d_tmp[15]}}, Ug_d_tmp[15:6] };
Delta_tmp <= Delta_tmp + 14'h39;
end
4'h7:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {7{Ug_q_tmp[15]}}, Ug_q_tmp[15:7] };
Ug_q_tmp <= Ug_q_tmp + { {7{Ug_d_tmp[15]}}, Ug_d_tmp[15:7] };
Delta_tmp <= Delta_tmp - 14'h1C;//(28/64=0.4375)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {7{Ug_q_tmp[15]}}, Ug_q_tmp[15:7] };
Ug_q_tmp <= Ug_q_tmp - { {7{Ug_d_tmp[15]}}, Ug_d_tmp[15:7] };
Delta_tmp <= Delta_tmp + 14'h1C;
end
4'h8:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {8{Ug_q_tmp[15]}}, Ug_q_tmp[15:8] };
Ug_q_tmp <= Ug_q_tmp + { {8{Ug_d_tmp[15]}}, Ug_d_tmp[15:8] };
Delta_tmp <= Delta_tmp - 14'hE;//(14/64=0.21875)
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {8{Ug_q_tmp[15]}}, Ug_q_tmp[15:8] };
Ug_q_tmp <= Ug_q_tmp - { {8{Ug_d_tmp[15]}}, Ug_d_tmp[15:8] };
Delta_tmp <= Delta_tmp + 14'hE;
end
4'h9:
if ( Ug_q_tmp[15] )
begin
Ug_d_tmp <= Ug_d_tmp - { {9{Ug_q_tmp[15]}}, Ug_q_tmp[15:9] };
Ug_q_tmp <= Ug_q_tmp + { {9{Ug_d_tmp[15]}}, Ug_d_tmp[15:9] };
Delta_tmp <= Delta_tmp - 14'h7;
end
else
begin
Ug_d_tmp <= Ug_d_tmp + { {9{Ug_q_tmp[15]}}, Ug_q_tmp[15:9] };
Ug_q_tmp <= Ug_q_tmp - { {9{Ug_d_tmp[15]}}, Ug_d_tmp[15:9] };
Delta_tmp <= Delta_tmp + 14'h7; //(7/64=0.109375)
end
default: //缺省情况下所有寄存器清零
begin
Ug_d_tmp <= 16'h0;
Ug_q_tmp <= 16'h0;
Delta_tmp <= 14'h0;
end
// ;
endcase
end
else
Times[3:0] <= 4'hf; //迭代计算完毕，结束CORDIC算法，迭代次数置复位值
end
endmodule
