feat: Add integrate and dump for StopToCurrent

* Correction output is integrated and dumped every 1024 values
* when corrector is disabled, switch output to averaged value

hdl/corr_ll.vhd

@@ -41,6 +41,7 @@ end entity corr_ll;
 architecture rtl of corr_ll is
 
     constant C_N_A_RND  : natural := 8;
+    constant C_INT      : natural := 10;
     constant C_DELAY    : natural := 5;
     type arr_slv is array (natural range <>) of std_logic_vector;
     signal delay_valid  : std_logic_vector(C_DELAY-1 downto 0);
@@ -99,6 +100,10 @@ begin
         signal enable_corr  : std_logic;
         signal reset_corr   : std_logic;
 
+        signal integrate_dout   : signed(C_W_COR+C_INT-1 downto 0);
+        signal avg_dout         : signed(C_W_COR-1 downto 0);
+        signal integrate_cnt    : unsigned(C_INT-1 downto 0);
+
     begin
 
         -- Signal mux for X/Y coef and enable/reset, based on PSCID number
@@ -133,6 +138,9 @@ begin
                 rz_mult_a       <= (others => '0');
                 rz_mult_b       <= (others => '0');
                 rz_mult_d       <= (others => '0');
+                integrate_dout  <= (others => '0');
+                integrate_cnt   <= (others => '0');
+                avg_dout        <= (others => '0');
             elsif rising_edge(clk) then
 
                 if reset_corr = '1' then
@@ -155,10 +163,26 @@ begin
 
                 if reset_corr = '1' then
                     reg_dout    <= (others => '0');
+                    integrate_dout  <= (others => '0');
+                    integrate_cnt   <= (others => '0');
+                    avg_dout        <= (others => '0');
                 else
-                    if delay_valid(delay_valid'left-1) = '1' and enable_corr = '1' then
-                        reg_dout        <= f_resize_sat(rnd_abicd, C_W_COR);
+                    if enable_corr = '1' then
+                        if delay_valid(delay_valid'left-1) = '1' then
+                            reg_dout        <= f_resize_sat(rnd_abicd, C_W_COR);
+                        end if;
+
+                        if delay_valid(3) = '1' then
+                            integrate_cnt   <= integrate_cnt-1;
+                            if integrate_cnt = 0 then
+                                avg_dout        <= integrate_dout(C_W_COR+C_INT-1 downto C_INT);
+                                integrate_dout  <= (others => '0');
+                            else
+                                integrate_dout  <= integrate_dout+reg_dout;
+                            end if;
+                        end if;
                     end if;
+
                 end if;
 
                 -- Round and resize
@@ -184,7 +208,8 @@ begin
                            f_sum_sat(f_resize_lsb(mult_ic, rnd_abicd'length), to_signed(1, rnd_abicd'length));
 
         -- mapping
-        corrout(I) <= reg_dout;
+        corrout(I) <= reg_dout when enable_corr='1' else
+                      avg_dout;
 
     end generate gen_corr;