Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library desy;
use desy.math_signed;
use work.pkg_corr_matrixpi.all;
entity corr_pi is
port(
clk : in std_logic;
rst_n : in std_logic;
-- matmult input
matmult : in signed_array(0 to C_N_MM_PSC-1)(C_W_MM_ACCU-1 downto 0);
matmult_valid : in std_logic;
matmult_seq : in std_logic_vector(C_W_SEQ-1 downto 0);
-- Corr coefs
corr_kp : signed(C_W_COR_KP-1 downto 0);
corr_ki : signed(C_W_COR_KI-1 downto 0);
reset_accu : in std_logic;
enable_corr : in std_logic;
-- Corr output
corrout_valid : out std_logic;
corrout_seq : out std_logic_vector(C_W_SEQ-1 downto 0);
corrout : out signed_array(0 to C_N_MM_PSC-1)(C_W_COR_OUT-1 downto 0)
);
end entity corr_pi;
architecture rtl of corr_pi is
type arr_slv is array (natural range <>) of std_logic_vector;
------------------------
-- SIGNAL DECLARATION --
------------------------
signal r_valid : std_logic_vector(2 downto 0);
signal r_seq : arr_slv(0 downto 2)(C_W_SEQ-1 downto 0);
begin
--------------------
-- DELAY REGISTER --
--------------------
p_delay:process(clk,rst_n)
begin
if rst_n = '0' then
r_valid <= (others => '0');
r_seq <= (others => (others => '0'));
elsif rising_edge(clk) then
r_valid <= r_valid(r_valid'left-1 downto 0) & matmult_valid;
r_seq(0) <= matmul_seq;
for I in 1 to r_seq'right loop
r_seq(I) <= r_seq(I-1);
end loop;
end if;
end process;
----------------------
-- OUTPUT CONNEXION --
----------------------
corrout_valid <= r_valid(2);
corrout_seq <= r_seq(2);
--------------------
-- CORRECTOR LINE --
--------------------
G_CORR:for I in 0 to C_N_MM_PSC-1 generate
signal r_matmult : signed(C_W_MM_ACCU-1 downto 0);
signal accu_pre : signed(C_W_MM_ACCU-1 downto 0);
signal accu_post : signed(C_W_COR_ACCU-1 downto 0);
signal ki_mult : signed(C_W_COR_KI+C_W_COR_ACCU-1 downto 0);
signal kp_mult : signed(C_W_COR_KP+C_W_COR_ACCU-1 downto 0);
signal ki_mult_rnd : signed(ki_mult'left-C_N_COR_KIRND downto 0);
signal corr_sum : signed(ki_mult_rnd'left downto 0);
signal corr_sum_rnd : signed(C_W_COR_OUT-1 downto 0);
begin
p_acc:process(clk, rst_n)
begin
if rst_n = '0' then
accu_pre <= (others => '0');
accu_post <= (others => '0');
ki_mult <= (others => '0');
kp_mult <= (others => '0');
corrout <= (others => (others => '0'));
elsif rising_edge(clk) then
-----------------
-- ACCUMULATOR --
-----------------
-- This is a bilinear integrator !
if matmult_valid = '1' then
accu_pre <= matmult(I);
end if;
if rst_accu = '1' then
accu_post <= (others => '0');
else
if ena_accu = '1' then
accu_post <= f_sum_sat(f_sum_sat(accu_post, matmult), accu_pre);
end if;
end if;
-- Delay
r_matmult <= matmult(I);
----------------------
-- COEFF MULTIPLIER --
----------------------
ki_mult <= accu_post * corr_ki;
kp_mult <= r_matmult * corr_kp;
--------------------------
-- FINAL ADDER REGISTER --
--------------------------
corrout(I) <= f_resize_sat(ki_mult_rnd, C_W_COR_OUT);
end if;
end process;
----------------------
-- KI MULT ROUNDING --
----------------------
ki_mult_rnd <= ki_mult(ki_mult'left downto C_N_COR_KIRND) when ki_mult(C_N_COR_KIRND-1) = '1' else
f_sum_sat(ki_mult(ki_mult'left downto C_N_COR_KIRND), to_signed(1,1));
-----------------------
-- FINAL ADDER LOGIC --
-----------------------
corr_sum <= f_sum_sat(ki_mult_rnd, kp_mult);
--------------------
-- FINAL ROUNDING --
--------------------
corr_sum_rnd <= corr_sum(corr_sum'left downto C_N_COR_RND) when corr_sum(C_N_COR_RND-1) = '1' else
f_sum_sat(corr_sum(corr_sum'left downto C_N_COR_RND), to_signed(1,1));
end generate;
end architecture;