/*
dsp48a1 x2 dsp48a1 x2
滤波器阶数 4 480n
抽取倍数 27x 5x
采样率 64.8M 2.4M 480k
带宽 200k 200k/可变
+-->mul----->+ +--->+ +------+ +---------------------------------------->+--------------
| | | | | | | | |
adc------->+ +<--+ cic ddc_fifo fir_comp_dec_iq |
| | | | | | | | |
+-->mul-- -->+ +--->+ +------+ +----------------------------------------- -->+----------
| | | |
| | phase_increase | |
| +<--+ +<----+ +<---phase_add<---loop_filter<------------+<---+ +--+ |
| | | | | | | | | | |
| cordic_dds dds_iq | +<---phase_diff<---+ arctan |
| | | | | | | | |
+<------+ +<----+ | | | +<-----+
| |
*/
/*
+---+
din--+->|mul|------------------------------->+
| +---+ |
| | |
| c1 |
| |
| +---+ +---+ +---+
+->|mul|--->|add|-->integral------+-->|add|-->dout
+---+ +---+ | +---+
| | |
c2 +<--integral_last<--+
*/
/*
+---+
phase_diff-->+--->|mul|----------------------------------------------------------->+
| +---+ |
| | |
| freq_c1 |
| |
| +---+ |
+--->|mul|------>+ |
+---+ | |
| | |
freq_c2 | +<---integral_0_last<---+ | +<---integral_1_last<---+
| | | | | |
+---+ +---+ +---+ | +---+ +---+ +---+ | sum_1_k1 +---+
phase----+------->|mul|---->|add|--->sum_c2_k3--->|add|--->integral_0------>+--->|add|--->sum_0_c1--->|add|--->sum_0_c1_k2--->|add|--->integral_1------>+------------->|add|--->dout--->
| +---+ +---+ +---+ +---+ +---+ +---+ +---+
| | | |
| phase_k3 | |
| | |
| +---+ | |
+------->|mul|-------------------------------------------------------------------------------->+ |
| +---+ |
| | |
| phase_k2 |
| |
| +---+ |
+------->|mul|--------------------------------------------------------------------------------------------------------------------------------------------------+
+---+
|
phase_k1
*/
module loop_filter
#(
parameter ANGLE_WIDTH=36
)
(
input rst,
input clk,
input signed [ANGLE_WIDTH - 1:0] phase,
input signed [ANGLE_WIDTH - 1:0] phase_diff,
input din_latch,
output reg signed [31:0] dout,
output reg dout_latch
);
////////////////////////////////////////////////////////////////
localparam shift_freq_c1 = 13;
localparam shift_freq_c2 = 19;
localparam shift_phase_k1 = 12;
localparam shift_phase_k2 = 17;
localparam shift_phase_k3 = 26;
localparam INTEGRAL_WIDTH = ANGLE_WIDTH + shift_phase_k3;
reg [INTEGRAL_WIDTH - 1:0] integral_0 = 0;
reg [INTEGRAL_WIDTH - 1:0] integral_0_last = 0;
reg [INTEGRAL_WIDTH - 1:0] integral_1 = 0;
reg [INTEGRAL_WIDTH - 1:0] integral_1_last = 0;
reg din_latch_last = 0;
reg din_latch_0 = 0;
reg din_latch_0_last = 0;
reg din_latch_1 = 0;
reg din_latch_1_last = 0;
reg signed [INTEGRAL_WIDTH - 1:0] sum_c2_k3 = 0;
reg signed [INTEGRAL_WIDTH - 1:0] sum_0_c1 = 0;
reg signed [INTEGRAL_WIDTH - 1:0] sum_0_c1_k2 = 0;
wire signed [INTEGRAL_WIDTH - 1:0] sum_1_k1;
reg rst_last = 0;
wire [INTEGRAL_WIDTH - 1:0] mul_freq_c1 = {{shift_freq_c1{phase_diff[ANGLE_WIDTH - 1]}},phase_diff, {(INTEGRAL_WIDTH - shift_freq_c1 - ANGLE_WIDTH){1'b0}}};
wire [INTEGRAL_WIDTH - 1:0] mul_freq_c2 = {{shift_freq_c2{phase_diff[ANGLE_WIDTH - 1]}},phase_diff, {(INTEGRAL_WIDTH - shift_freq_c2 - ANGLE_WIDTH){1'b0}}};
wire [INTEGRAL_WIDTH - 1:0] mul_phase_k1 = {{shift_phase_k1{phase[ANGLE_WIDTH - 1]}}, phase, {(INTEGRAL_WIDTH - shift_phase_k1 - ANGLE_WIDTH){1'b0}}};
wire [INTEGRAL_WIDTH - 1:0] mul_phase_k2 = {{shift_phase_k2{phase[ANGLE_WIDTH - 1]}}, phase, {(INTEGRAL_WIDTH - shift_phase_k2 - ANGLE_WIDTH){1'b0}}};
wire [INTEGRAL_WIDTH - 1:0] mul_phase_k3 = {{shift_phase_k3{phase[ANGLE_WIDTH - 1]}}, phase, {(INTEGRAL_WIDTH - shift_phase_k3 - ANGLE_WIDTH){1'b0}}};
/////////////////////////////////////
always @(posedge clk)
begin
if(din_latch)
sum_c2_k3 <= mul_freq_c2 - mul_phase_k3;
din_latch_last <= din_latch;
end
always @(negedge clk)
begin
if(rst)
begin
integral_0 <= 0;
end
else
begin
if(din_latch_last)
begin
integral_0 <= integral_0_last + sum_c2_k3;
end
end
din_latch_0 <= din_latch_last;
end
always @(posedge clk)
begin
integral_0_last <= integral_0;
end
always @(posedge clk)
begin
if(din_latch_0)
sum_0_c1 <= integral_0 + mul_freq_c1;
din_latch_0_last <= din_latch;
end
always @(negedge clk)
begin
if(din_latch_0_last)
sum_0_c1_k2 <= sum_0_c1 - mul_phase_k2;
din_latch_1 <= din_latch_0_last;
rst_last <= rst;
end
//////////////////////////////////////////////////////////////
always @(posedge clk)
begin
if(rst_last)
begin
integral_1 <= 0;
end
else
begin
if(din_latch_1)
begin
integral_1 <= integral_1_last + sum_0_c1_k2;
end
end
din_latch_1_last <= din_latch_1;
end
always @(negedge clk)
begin
integral_1_last <= integral_1;
end
assign sum_1_k1 = integral_1 - mul_phase_k1;
always @(negedge clk)
begin
if(rst)
begin
dout <= 0;
end
else
begin
if(din_latch_1_last)
dout <= sum_1_k1[(INTEGRAL_WIDTH - 1)-:32];
end
dout_latch <= din_latch_1_last;
end
endmodule
/*
module loop_filter__freq
#(
parameter ANGLE_WIDTH=36
)
(
input rst,
input costas_en,
input clk,
input signed [ANGLE_WIDTH - 1:0] din,
input din_latch,
output reg signed [31:0] dout,
output reg dout_latch
);
////////////////////////////////////////////////////////////////
localparam shift_c1 = 13;
localparam shift_c2 = 19;
localparam shift_c3 = 24;
localparam INTEGRAL_WIDTH = ANGLE_WIDTH + shift_c3;
reg [INTEGRAL_WIDTH - 1:0] integral_0 = 0;
reg [INTEGRAL_WIDTH - 1:0] integral_0_last = 0;
reg [INTEGRAL_WIDTH - 1:0] integral_1 = 0;
reg [INTEGRAL_WIDTH - 1:0] integral_1_last = 0;
reg din_latch_last = 0;
reg costas_en_last = 0;
reg [INTEGRAL_WIDTH - 1:0] sum_0;
wire [INTEGRAL_WIDTH - 1:0] sum_1;
wire [INTEGRAL_WIDTH - 1:0] mul_c1 = {{shift_c1{din[ANGLE_WIDTH - 1]}},din,{(INTEGRAL_WIDTH - shift_c1 - ANGLE_WIDTH){1'b0}}};
wire [INTEGRAL_WIDTH - 1:0] mul_c2 = {{shift_c2{din[ANGLE_WIDTH - 1]}},din,{(INTEGRAL_WIDTH - shift_c2 - ANGLE_WIDTH){1'b0}}};
wire [INTEGRAL_WIDTH - 1:0] mul_c3 = {{shift_c3{din[ANGLE_WIDTH - 1]}},din,{(INTEGRAL_WIDTH - shift_c3 - ANGLE_WIDTH){1'b0}}};
/////////////////////////////////////
always @(posedge clk)
begin
din_latch_last <= din_latch;
costas_en_last <= costas_en;
end
always @(posedge clk)
begin
if(din_latch)
begin
integral_0 <= integral_0_last + mul_c2;
end
end
always @(negedge clk)
begin
if(rst || !costas_en_last)
integral_0_last <= 0;
else
integral_0_last <= integral_0;
end
always @(negedge clk)
begin
sum_0 <= integral_0 + mul_c1;
end
//////////////////////////////////////////////////////////////
always @(posedge clk)
begin
if(din_latch)
begin
integral_1 <= integral_1_last + sum_0;
end
end
always @(negedge clk)
begin
if(rst || !costas_en_last)
integral_1_last <= 0;
else
integral_1_last <= integral_1;
end
assign sum_1 = integral_1 + mul_c3;
always @(negedge clk)
begin
dout <= sum_1[(INTEGRAL_WIDTH - 1)-:32];
dout_latch <= din_latch_last;
end
endmodule
module loop_filter__phase
#(
parameter ANGLE_WIDTH=36
)
(
input rst,
input costas_en,
input clk,
input signed [ANGLE_WIDTH - 1:0] din,
input din_latch,
output reg signed [31:0] dout,
output reg dout_latch
);
////////////////////////////////////////////////////////////////
localparam shift_c1 = 8;
localparam shift_c2 = 13;
localparam INTEGRAL_WIDTH = ANGLE_WIDTH + shift_c2;
reg [INTEGRAL_WIDTH - 1:0] integral_0 = 0;
reg [INTEGRAL_WIDTH - 1:0] integral_0_last = 0;
reg din_latch_last = 0;
reg costas_en_last = 0;
wire [INTEGRAL_WIDTH - 1:0] sum_0;
wire [INTEGRAL_WIDTH - 1:0] mul_c1 = {{shift_c1{din[ANGLE_WIDTH - 1]}},din,{(INTEGRAL_WIDTH - shift_c1 - ANGLE_WIDTH){1'b0}}};
wire [INTEGRAL_WIDTH - 1:0] mul_c2 = {{shift_c2{din[ANGLE_WIDTH - 1]}},din,{(INTEGRAL_WIDTH - shift_c2 - ANGLE_WIDTH){1'b0}}};
reg rst_last = 0;
/////////////////////////////////////
always @(posedge clk)
begin
din_latch_last <= din_latch;
costas_en_last <= costas_en;
end
always @(posedge clk)
begin
if(rst_last)
begin
integral_0 <= 0;
end
else
begin
if(din_latch)
begin
integral_0 <= integral_0_last + mul_c2;
end
end
end
always @(negedge clk)
begin
if(rst || !costas_en_last)
integral_0_last <= 0;
else
integral_0_last <= integral_0;
rst_last <= rst;
end
assign sum_0 = integral_0 + mul_c1;
always @(negedge clk)
begin
if(rst)
begin
dout <= 0;
dout_latch <= 1'b0;
end
else
begin
if(din_latch_last)
begin
dout <= sum_0[(INTEGRAL_WIDTH - 1)-:32];
end
dout_latch <= din_latch_last;
end
end
endmodule
*/
module phase_diff
(
input clk,
input en,
input signed [35:0] phase_in,
input latch_in,
output reg signed [35:0] diff_out,
output reg latch_out
);
reg signed [35:0] phase_0 = 0;
reg signed [35:0] phase_0_last = 0;
reg signed [35:0] phase_1 = 0;
always @(posedge clk)
begin
if(latch_in)
phase_0 <= phase_in;
end
always @(negedge clk)
begin
phase_0_last <= phase_0;
end
always @(posedge clk)
begin
if(latch_in)
phase_1 <= phase_0_last;
end
always @(negedge clk)
begin
diff_out <= phase_1 - phase_0;
if(en)
latch_out <= latch_in;
else
latch_out <= 1'b0;
end
endmodule
module phase_detect_start_delay
#(
localparam DELAY = 12
)
(
input rst,
input clk,
input din_latch,
output reg dout_latch,
output reg diff_en
);
reg [3:0] count = 0;
reg [3:0] count_next = 0;
reg condition_next = 0;
reg din_latch_last = 0;
reg diff_en_0 = 0;
reg diff_en_0_last = 0;
always @(posedge clk)
begin
if(rst)
begin
count <= 0;
condition_next <= 1'b1;
end
else
begin
if(din_latch)
begin
count <= count_next;
condition_next <= count_next < DELAY;
end
end
din_latch_last <= din_latch;
end
always @(negedge clk)
begin
if(condition_next)
begin
count_next <= count + 1'b1;
end
if(condition_next)
dout_latch <= 1'b0;
else
dout_latch <= din_latch_last;
end
always @(posedge clk)
begin
if(rst)
begin
diff_en_0 <= 1'b0;
end
else
begin
if(din_latch)
diff_en_0 <= !condition_next;
end
end
always @(negedge clk)
begin
diff_en_0_last <= diff_en_0;
end
always @(posedge clk)
begin
if(din_latch)
diff_en <= diff_en_0_last;
end
endmodule
module lock_detect
#(
parameter HYSTERESIS_UP = 20,
parameter HYSTERESIS_DOWN = 20,
parameter DIFF_ABS = 2**24 - 1,
parameter HYSTERESIS_WIDTH = 8
)
(
input rst,
input clk,
input signed [35:0] din,
input din_latch,
input phase_update,
output reg lockon,
output reg latch_out
);
////////////////////////////////////////////////////////////////
reg [HYSTERESIS_WIDTH - 1:0] count_add = 0;
reg [HYSTERESIS_WIDTH - 1:0] count_sub = 0;
reg [HYSTERESIS_WIDTH - 1:0] count_add_next = 0;
reg [HYSTERESIS_WIDTH - 1:0] count_sub_next = 0;
reg in_range = 0;
reg din_latch_last = 0;
reg latch_0 = 0;
reg condition_next_add = 0;
reg condition_next_sub = 0;
////////////////////////////////////////////////////////////////
always @(posedge clk)
begin
if(rst || phase_update)
begin
in_range <= 1'b0;
end
else
begin
if(din_latch)
begin
if(din[35])
in_range <= (din > (0 - DIFF_ABS));
else
in_range <= (din < DIFF_ABS);
end
end
din_latch_last <= din_latch;
end
always @(negedge clk)
begin
if(in_range)
begin
if(condition_next_add)
count_add_next <= count_add + 1'b1;
else
count_sub_next <= HYSTERESIS_DOWN;
end
else
begin
if(condition_next_sub)
count_sub_next <= count_sub - 1'b1;
else
count_add_next <= 0;
end
latch_0 <= din_latch_last;
///////////////////////////////////////////
if(!condition_next_sub) // count_next_sub==0,fully unlock
begin
latch_out <= din_latch_last;
end
else
begin
if(in_range) // locked,and in_range
begin
latch_out <= din_latch_last;
end
end
end
always @(posedge clk)
begin
if(rst || phase_update)
begin
count_add <= 0;
count_sub <= 0;
condition_next_add <= 1'b1;
condition_next_sub <= 1'b0;
end
else
begin
if(latch_0)
begin
count_add <= count_add_next;
count_sub <= count_sub_next;
condition_next_add <= (count_add_next < HYSTERESIS_UP);
condition_next_sub <= (count_sub_next > 0);
end
end
end
always @(negedge clk)
begin
lockon <= condition_next_sub;
end
////////////////////////////////////////
endmodule
/*
module costas_adjust
(
input rst,
input clk,
input freq_lockon,
input phase_lockon,
input phase_update,
input signed [35:0] phase_in,
input signed [35:0] phase_diff_in,
input latch_in,
output reg signed [35:0] phase_out,
output reg signed [35:0] phase_diff_out,
output reg latch_out
);
////////////////////////////////////////////////////////////////
reg freq_lockon_last = 0;
reg freq_lockon_0 = 0;
//reg freq_unlocked = 0;
reg freq_relocked = 0;
////////////////////////////////////////////////////////////////
reg phase_lockon_last = 0;
reg phase_lockon_0 = 0;
reg phase_unlocked = 0;
//reg phase_relocked = 0;
////////////////////////////////////////////////////////////////
reg use_pll = 0;
reg phase_update_last = 0;
reg latch_in_last = 0;
////////////////////////////////////////////////////////////////
always @(posedge clk)
begin
freq_lockon_last <= freq_lockon;
end
always @(negedge clk)
begin
freq_lockon_0 <= freq_lockon_last;
end
always @(posedge clk)
begin
//freq_unlocked <= ({freq_lockon_0,freq_lockon} == 2'b10);
freq_relocked <= ({freq_lockon_0,freq_lockon} == 2'b01);
end
////////////////////////////////////////////////////////////////
always @(posedge clk)
begin
phase_lockon_last <= phase_lockon;
end
always @(negedge clk)
begin
phase_lockon_0 <= phase_lockon_last;
end
always @(posedge clk)
begin
phase_unlocked <= ({phase_lockon_0,phase_lockon} == 2'b10);
//phase_relocked <= ({phase_lockon_0,phase_lockon} == 2'b01);
end
/////////////////////////////////////////////////////////////////
always @(posedge clk)
begin
phase_update_last <= phase_update;
end
always @(negedge clk)
begin
if(rst || phase_update_last)// || phase_unlocked)
begin
use_pll <= 1'b0;
end
else
begin
if(freq_relocked)
use_pll <= 1'b1;
end
end
always @(posedge clk)
begin
if(use_pll)
begin
phase_out <= phase_in;
phase_diff_out <= 0;
end
else
begin
phase_out <= 0;
phase_diff_out <= phase_diff_in;
end
latch_in_last <= latch_in;
end
always @(negedge clk)
begin
latch_out <= latch_in_last;
end
endmodule
*/
module costas_loop
#(
parameter RAM_ADDR_WIDTH = 8
)
(
input rst,
input costas_en,
input adc_clk,
input signed [17:0] adc_d,
input [31:0] phase_increase_base,
input phase_update,
input proc_clk,
output [5:0] error,
input coef_in_clk,
input coef_in_we,
input [RAM_ADDR_WIDTH - 1:0] coef_in_addr,
input signed [17:0] coef_in_d,
output signed [17:0] ddc_out_i,
output signed [17:0] ddc_out_q,
output ddc_out_latch,
output signed [35:0] dout_angle,
output [17:0] magnitude,
output lockon,
output reg [31:0] phase_increase
);
//////////////////////////////////////////////////////////////
wire signed [35:0] phase_detect_out;
wire phase_detect_out_latch;
wire signed [35:0] phase_diff_out;
wire phase_diff_out_latch;
wire signed [31:0] loop_filter_out;
wire loop_filter_out_latch;
wire [4:0] error_ddc;
wire error__arctan__conflict;
assign error = {error__arctan__conflict,error_ddc};
wire phase_detect_start;
wire diff_en;
wire freq_lockon;
wire freq_lockon_latch;
wire phase_lockon;
wire phase_lockon_latch;
assign dout_angle = phase_detect_out;
assign lockon = phase_lockon;
reg signed [31:0] loop_filter_out_last = 0;
wire signed [35:0] adjust_phase_out;
wire signed [35:0] adjust_phase_diff_out;
wire adjust_latch_out;
ddc
#(
.RAM_ADDR_WIDTH (RAM_ADDR_WIDTH)
)
ddc_ins
(
.rst (rst),
.adc_clk (adc_clk),
.adc_d (adc_d),
.phase_increase (phase_increase),
.phase_offset (32'd0),
.proc_clk (proc_clk),
.error (error_ddc),
.coef_in_clk (coef_in_clk),
.coef_in_we (coef_in_we),
.coef_in_addr (coef_in_addr),
.coef_in_d (coef_in_d),
.dout_i (ddc_out_i),
.dout_q (ddc_out_q),
.dout_latch (ddc_out_latch)
);
phase_detect_start_delay start_delay
(
.rst (rst),
.clk (proc_clk),
.din_latch (ddc_out_latch),
.dout_latch (phase_detect_start),
.diff_en (diff_en)
);
/*
cordic_arctan phase_detect
(
.rst (rst),
.clk (proc_clk),
.calc_begin (phase_detect_start),
.error_conflict (error__arctan__conflict),
.din_x (ddc_out_q),
.din_y (ddc_out_i),
.dout_angle (phase_detect_out),
.dout_latch (phase_detect_out_latch)
);
*/
cordic_arctan_and_magnitude phase_detect
(
.rst (rst),
.clk (proc_clk),
.calc_begin (phase_detect_start),
.error_conflict (error__arctan__conflict),
.din_x (ddc_out_q),
.din_y (ddc_out_i),
.dout_angle (phase_detect_out),
.magnitude (magnitude),
.dout_latch (phase_detect_out_latch)
);
phase_diff phase_diff_ins
(
.clk (proc_clk),
.en (diff_en),
.phase_in (phase_detect_out),
.latch_in (phase_detect_out_latch),
.diff_out (phase_diff_out),
.latch_out (phase_diff_out_latch)
);
lock_detect
#(
.HYSTERESIS_UP (20),
.HYSTERESIS_DOWN (20),
.DIFF_ABS (2**24 - 1),
.HYSTERESIS_WIDTH (6)
)
freq_lock_detect
(
.rst (rst),
.clk (proc_clk),
.din (phase_diff_out),
.din_latch (phase_diff_out_latch),
.phase_update (phase_update),
.lockon (freq_lockon),
.latch_out (freq_lockon_latch)
);
lock_detect
#(
.HYSTERESIS_UP (50),
.HYSTERESIS_DOWN (50),
.DIFF_ABS (2**27 - 1),
.HYSTERESIS_WIDTH (6)
)
phase_lock_detect
(
.rst (rst),
.clk (proc_clk),
.din (phase_detect_out),
.din_latch (phase_detect_out_latch),
.phase_update (phase_update),
.lockon (phase_lockon),
.latch_out (phase_lockon_latch)
);
/*
costas_adjust adjust
(
.rst (rst),
.clk (proc_clk),
.freq_lockon (freq_lockon),
.phase_lockon (phase_lockon),
.phase_update (phase_update),
.phase_in (phase_detect_out),
.phase_diff_in (phase_diff_out),
.latch_in (freq_lockon_latch),
.phase_out (adjust_phase_out),
.phase_diff_out (adjust_phase_diff_out),
.latch_out (adjust_latch_out)
);
*/
loop_filter loop_filter_ins
(
.rst (rst),
.clk (proc_clk),
.phase (phase_detect_out), //(adjust_phase_out),
.phase_diff (phase_diff_out), //(adjust_phase_diff_out),
.din_latch (phase_diff_out_latch), //(adjust_latch_out),
.dout (loop_filter_out),
.dout_latch (loop_filter_out_latch)
);
/*
costas_unlock_detect unlock_detect
(
.rst (rst),
.clk (proc_clk),
.locked (locked),
.phase_update (phase_update),
.adjust (adjust),
.adjust_out (adjust_final)
);
*/
always @(posedge proc_clk)
begin
if(loop_filter_out_latch)
loop_filter_out_last <= loop_filter_out;
end
always @(negedge proc_clk)
begin
if(costas_en)
phase_increase <= phase_increase_base + loop_filter_out_last;
else
phase_increase <= phase_increase_base;
end
endmodule
costas_loop
标签：+---+,INTEGRAL,WIDTH,shift,phase,costas,reg,loop From： https://www.cnblogs.com/jacob1934/p/18301116