feat(average): Add orbit error and correction command moving average

* Same generic bloc for operations
* Capture the stream of data and retains accumulator in memory
* Mirror memory for CPU readout

wip: initial commit

wip: Now a more generic bloc

wip: Add average tables to RDL

wip: integrate ma for oe in toplevel

wip: first corrections for simulation, alpha declared in package

wip: fixes for simulation

wip: fixes, secondary memory is smaller, no decimal part

wip: true rounding before wrinting to secondary memory

wip: Add average for PSC command

wip: fix sign extension on average orbit read

hdl/moving_average.vhd

+-- Block that compute a moving average from a AXIStream of data with ID
+-- Results are published in a memory accessible from AXI-MM
+library ieee;
+use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
+
+library desy;
+use desy.ram_tdp;
+use desy.math_signed.all;
+
+entity moving_average is
+    generic(
+        G_W_ID          : natural;
+        G_W_SIGNAL      : natural;
+        G_W_ALPHA       : natural
+    );
+    port(
+        clk             : in std_logic;
+        rst_n           : in std_logic;
+
+        -- Moving average alpha
+        alpha           : in signed(G_W_ALPHA-1 downto 0);
+
+        -- Signal input
+        sig_data        : in signed(G_W_SIGNAL-1 downto 0);
+        sig_id          : in std_logic_vector(G_W_ID-1 downto 0);
+        sig_valid       : in std_logic;
+
+        -- AXI-MM Average signal table
+        asigt_en        : in std_logic;
+        asigt_addr      : in std_logic_vector(G_W_ID-1 downto 0);
+        asigt_rdata     : out std_logic_vector(G_W_SIGNAL-1 downto 0)
+    );
+end entity moving_average;
+
+architecture rtl of moving_average is
+
+    type arr_slv is array (natural range <>) of std_logic_vector;
+
+    ------------------------
+    -- SIGNAL DECLARATION --
+    ------------------------
+    signal table_wen        : std_logic;
+    signal table_waddr      : std_logic_vector(G_W_ID-1 downto 0);
+    signal table_wdata      : std_logic_vector(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+    signal table_wdata_sec  : std_logic_vector(G_W_SIGNAL-1 downto 0);
+    signal table_rdata      : std_logic_vector(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+
+    signal sig_id_r         : arr_slv(0 to 2)(G_W_ID-1 downto 0);
+    signal sig_valid_r      : std_logic_vector(0 to 2);
+
+    signal pasig            : signed(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+
+    signal sig_alpha        : signed(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+    signal sig_alpha_r      : signed(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+
+    signal pasig_alpha      : signed(G_W_SIGNAL+G_W_ALPHA*2-1 downto 0);
+    signal pasig_alpha_rnd  : signed(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+    signal ma_update        : signed(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+    signal ma_new           : signed(G_W_SIGNAL+G_W_ALPHA-1 downto 0);
+    signal ma_new_rnd       : signed(G_W_SIGNAL-1 downto 0);
+
+
+begin
+
+
+    --------------------------------
+    -- AVERAGE ORBIT ERROR TABLES --
+    --------------------------------
+    -- main one, used for computation. A reads only, B writes only
+
+    inst_asig_main_table: entity desy.ram_tdp
+    generic map(
+        G_ADDR      => G_W_ID,
+        G_DATA      => G_W_SIGNAL+G_W_ALPHA
+    )
+    port map(
+        pi_clk_a    => clk,
+        pi_en_a     => '1',
+        pi_we_a     => '0',
+        pi_addr_a   => sig_id,
+        pi_data_a   => (others => '0'),
+        po_data_a   => table_rdata,
+        pi_clk_b    => clk,
+        pi_en_b     => '1',
+        pi_we_b     => table_wen,
+        pi_addr_b   => table_waddr,
+        pi_data_b   => table_wdata,
+        po_data_b   => open
+    );
+    table_wdata <= std_logic_vector(ma_new);
+
+    -- secondary one, used for axi access to result. A reads only, B writes only
+    -- B port is mirror of previous bloc
+    -- No need to retain decimal part
+
+    inst_asig_sec_table: entity desy.ram_tdp
+    generic map(
+        G_ADDR      => G_W_ID,
+        G_DATA      => G_W_SIGNAL
+    )
+    port map(
+        pi_clk_a    => clk,
+        pi_en_a     => asigt_en,
+        pi_we_a     => '0',
+        pi_addr_a   => asigt_addr,
+        pi_data_a   => (others => '0'),
+        po_data_a   => asigt_rdata,
+        pi_clk_b    => clk,
+        pi_en_b     => '1',
+        pi_we_b     => table_wen,
+        pi_addr_b   => table_waddr,
+        pi_data_b   => table_wdata_sec,
+        po_data_b   => open
+    );
+
+    -- Round before memorize
+    table_wdata_sec <= std_logic_vector(ma_new_rnd);
+    ma_new_rnd      <= f_resize_lsb(ma_new, ma_new_rnd'length) when ma_new(G_W_ALPHA-1) = '0' else
+                       f_sum_sat(f_resize_lsb(ma_new, ma_new_rnd'length), to_signed(1, ma_new_rnd'length));
+
+
+    ------------------------
+    -- PIPELINE REGISTERS --
+    ------------------------
+    p_pipe:process(clk, rst_n)
+    begin
+        if rst_n = '0' then
+            sig_id_r         <= (others => (others => '0'));
+            sig_valid_r      <= (others => '0');
+        elsif rising_edge(clk) then
+
+            sig_valid_r <= sig_valid & sig_valid_r(0 to sig_valid_r'right-1);
+
+            sig_id_r(0) <= sig_id;
+            for I in 1 to sig_id_r'right loop
+                sig_id_r(I) <= sig_id_r(I-1);
+            end loop;
+
+        end if;
+    end process;
+
+    table_wen       <= sig_valid_r(2);
+    table_waddr     <= sig_id_r(2);
+
+
+    -----------------
+    -- COMPUTATION --
+    -----------------
+    pasig                <= signed(table_rdata);
+    pasig_alpha          <= -pasig * alpha;
+    pasig_alpha_rnd      <= f_resize_lsb(pasig_alpha, pasig_alpha_rnd'length) when pasig_alpha(G_W_ALPHA-1) = '0' else
+                           f_sum_sat(f_resize_lsb(pasig_alpha, pasig_alpha_rnd'length), to_signed(1, pasig_alpha_rnd'length));
+
+    p_comp:process(clk, rst_n)
+    begin
+        if rst_n = '0' then
+            sig_alpha        <= (others => '0');
+            sig_alpha_r      <= (others => '0');
+            ma_update        <= (others => '0');
+            ma_new           <= (others => '0');
+
+        elsif rising_edge(clk) then
+
+            sig_alpha        <= sig_data * alpha;
+            sig_alpha_r      <= sig_alpha;
+
+            ma_update        <= f_sum_sat(pasig, pasig_alpha_rnd);
+            ma_new           <= f_sum_sat(sig_alpha_r, ma_update);
+
+        end if;
+    end process;
+
+
+
+end architecture;
+

hdl/pkg_corrmatrix.vhd

@@ -46,6 +46,11 @@ package pkg_corr_matrix is
     -- Serializer
     constant C_W_SER_CNT        : natural := 7; -- natural(ceil(log2(real(C_N_MM_PSC))));
 
+    -- Moving average
+    constant C_W_ALPHA          : natural := 16;
+    constant C_ALPHA_OE         : signed(C_W_ALPHA-1 downto 0) := x"0048"; -- 72 dec
+    constant C_ALPHA_CC         : signed(C_W_ALPHA-1 downto 0) := x"0048"; -- 72 dec
+
     ----------------------
     -- TYPE DECLARATION --
     ----------------------

hdl/top_corr_matrix.vhd

@@ -136,6 +136,60 @@ begin
         errbpm_tvalid          => errbpm_tvalid
     );
 
+    -------------------------
+    -- ORBIT ERROR AVERAGE --
+    -------------------------
+    inst_orbit_error_avg_x: entity work.moving_average
+    generic map(
+        G_W_ID          => C_W_MM_IDCNT,
+        G_W_SIGNAL      => C_W_OE,
+        G_W_ALPHA       => C_W_ALPHA
+    )
+    port map(
+        clk             => clk,
+        rst_n           => rst_n,
+
+        -- Moving average alpha
+        alpha           => C_ALPHA_OE,
+
+        -- Signal input
+        sig_data        => errbpm_x,
+        sig_id          => errbpm_id(C_W_MM_IDCNT-1 downto 0),
+        sig_valid       => errbpm_tvalid,
+
+        -- AXI-MM Average signal table
+        asigt_en        => mm_a2l.ERRORBITX.en,
+        asigt_addr      => mm_a2l.ERRORBITX.addr(C_W_MM_IDCNT-1 downto 0),
+        asigt_rdata     => mm_l2a.ERRORBITX.data(C_W_OE-1 downto 0)
+    );
+    mm_l2a.ERRORBITX.data(31 downto C_W_OE) <= (others => mm_l2a.ERRORBITX.data(C_W_OE-1));
+
+    inst_orbit_error_avg_y: entity work.moving_average
+    generic map(
+        G_W_ID          => C_W_MM_IDCNT,
+        G_W_SIGNAL      => C_W_OE,
+        G_W_ALPHA       => C_W_ALPHA
+    )
+    port map(
+        clk             => clk,
+        rst_n           => rst_n,
+
+        -- Moving average alpha
+        alpha           => C_ALPHA_OE,
+
+        -- Signal input
+        sig_data        => errbpm_y,
+        sig_id          => errbpm_id(C_W_MM_IDCNT-1 downto 0),
+        sig_valid       => errbpm_tvalid,
+
+        -- AXI-MM Average signal table
+        asigt_en        => mm_a2l.ERRORBITY.en,
+        asigt_addr      => mm_a2l.ERRORBITY.addr(C_W_MM_IDCNT-1 downto 0),
+        asigt_rdata     => mm_l2a.ERRORBITY.data(C_W_OE-1 downto 0)
+    );
+    mm_l2a.ERRORBITY.data(31 downto C_W_OE) <= (others => mm_l2a.ERRORBITY.data(C_W_OE-1));
+
+
     ---------------------------
     -- MATRIX MULTIPLICATION --
     ---------------------------
@@ -251,6 +305,34 @@ begin
     m_axis_tuser    <= ser_tuser;
     m_axis_tvalid   <= ser_tvalid;
 
+    -------------------------------
+    -- CORRECTOR COMMAND AVERAGE --
+    -------------------------------
+    inst_corrector_command_avg: entity work.moving_average
+    generic map(
+        G_W_ID          => C_W_PSCID,
+        G_W_SIGNAL      => C_W_COR,
+        G_W_ALPHA       => C_W_ALPHA
+    )
+    port map(
+        clk             => clk,
+        rst_n           => rst_n,
+
+        -- Moving average alpha
+        alpha           => C_ALPHA_CC,
+
+        -- Signal input
+        sig_data        => ser_tdata(C_W_COR-1 downto 0),
+        sig_id          => ser_tdata(C_W_PSCID+C_W_COR-1 downto C_W_COR),
+        sig_valid       => ser_tvalid,
+
+        -- AXI-MM Average signal table
+        asigt_en        => mm_a2l.CORRCMD.en,
+        asigt_addr      => mm_a2l.CORRCMD.addr(C_W_PSCID-1 downto 0),
+        asigt_rdata     => mm_l2a.CORRCMD.data(C_W_COR-1 downto 0)
+    );
+    mm_l2a.CORRCMD.data(31 downto C_W_COR ) <= (others => mm_l2a.CORRCMD.data(C_W_COR-1));
+
     --------------------------
     -- THRESHOLD LEVEL COMP --
     --------------------------

rdl/corr_matrix.rdl

@@ -113,6 +113,27 @@ addrmap corr_matrix {
         mementries = 2**`C_W_MM_IDCNT;
     } external MATRIXCOEF[`C_N_MM_PSC];
 
+    mem {
+        desc = "X Average orbit error";
+        sw=r;
+        memwidth = 32;
+        mementries = 2**`C_W_MM_IDCNT;
+    } external ERRORBITX;
+
+    mem {
+        desc = "X Average orbit error";
+        sw=r;
+        memwidth = 32;
+        mementries = 2**`C_W_MM_IDCNT;
+    } external ERRORBITY;
+
+    mem {
+        desc = "Average correction, both planes";
+        sw=r;
+        memwidth = 32;
+        mementries = 2**`C_W_PSCID;
+    } external CORRCMD;
+
 };
 
 

tcl/main.tcl

@@ -24,6 +24,7 @@ proc setSources {} {
   lappend Sources {"../hdl/matrix_mul.vhd" "VHDL 2008"}
   lappend Sources {"../hdl/orbit_error.vhd" "VHDL 2008"}
   lappend Sources {"../hdl/data_serializer.vhd" "VHDL 2008"}
+  lappend Sources {"../hdl/moving_average.vhd" "VHDL 2008"}
   lappend Sources {"../hdl/top_corr_matrix.vhd" "VHDL 2008"}
   lappend Sources [list "${::fwfwk::LibPath}/desy_vhdl/hdl/memory/ram/ram_tdp.vhd" "VHDL 2008" "desy"]
   lappend Sources [list "${::fwfwk::LibPath}/desy_vhdl/hdl/math/pkg_math_utils.vhd" "VHDL 2008" "desy"]