Add moving average on orbit error and corrector command

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

fix(sim): Fiw simulation to match two correctors, add traces for moving
average

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        => signed(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;
+
 };
 
 

sim/compute_moving_avg.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Mon Oct 30 11:24:47 2023
+
+@author: broucquart
+"""
+
+import pandas as pd
+import numpy as np
+
+
+inputs = pd.read_csv("bpmdata.txt", header=None, sep=' ')
+inputs.drop(columns=244, inplace=True)
+_c=np.empty(244, dtype=int)
+_c[::2] = range(1,123)
+_c[1::2] = range(123,245)
+inputs.columns = _c
+
+
+orbrefs = pd.read_csv("reforbit.txt", header=None)[0]
+orbrefs.index = orbrefs.index+1
+
+
+alpha=72
+
+MA = inputs.copy()
+MA.loc[-1]=0
+
+for n in range(len(MA)-1): 
+    MA.loc[n] = (inputs.loc[n]-orbrefs)*alpha + MA.loc[n-1] - ( (MA.loc[n-1]*alpha).apply(lambda x: np.round(x/2**16,0).astype('int')) )
+    
+
+rMA = MA.apply(lambda x: np.round(x/2**16,0).astype('int'))

sim/generate_datasim.py

@@ -65,13 +65,21 @@ with open("bpmdata.txt", "w") as fp:
 trespmat = np.zeros((N_PSC, N_BPM), dtype="int64")
 trespmat[:50,:122] = respmat[:50]
 trespmat[50:,122:] = respmat[50:]
+C_N_RND = 20
+SUMSAT=36
+
 
 K_A = 218
 K_B = -186
 K_iC = 325
 K_D = -3225
-C_N_RND = 26
-SUMSAT=36
+
+
+K_A = 1024
+K_B = 0
+K_iC = 1024
+K_D = 0
+
 
 ## -----------------------
 # Model computation

sim/tc_basic.vhd

@@ -88,14 +88,23 @@ begin
 
         log("==--- Configure the DUT ---==", INFO);
         log("+-- Global Config", INFO);
+
         -- Correction coefficients
-        Write(ManagerRec, f_addr(16#0C#), f_sdata(218));
-        Write(ManagerRec, f_addr(16#10#), f_sdata(-186));
-        Write(ManagerRec, f_addr(16#14#), f_sdata(325));
-        Write(ManagerRec, f_addr(16#18#), f_sdata(-3225));
+        Write(ManagerRec, f_addr(16#10#), f_sdata(218));
+        Write(ManagerRec, f_addr(16#14#), f_sdata(-186));
+        Write(ManagerRec, f_addr(16#18#), f_sdata(325));
+        Write(ManagerRec, f_addr(16#1C#), f_sdata(-3225));
+
+        Write(ManagerRec, f_addr(16#20#), f_sdata(128));
+        Write(ManagerRec, f_addr(16#24#), f_sdata(0));
+        Write(ManagerRec, f_addr(16#28#), f_sdata(8192));
+        Write(ManagerRec, f_addr(16#2C#), f_sdata(0));
+
+        -- Rst corr and threshold
+        Write(ManagerRec, f_addr(8), f_sdata(10));
 
         -- Enable
-        Write(ManagerRec, f_addr(8), f_sdata(5));
+        Write(ManagerRec, f_addr(8), f_sdata(1));
 
         log("+-- Writing orbit reference...", INFO);
 

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"]