Skip to content
Snippets Groups Projects
Commit 90d4abe7 authored by BRONES Romain's avatar BRONES Romain
Browse files

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
parents 4c2da9ce 5a8769f2
No related branches found
Tags 1.2.0
No related merge requests found
-- 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;
......@@ -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 --
----------------------
......
......@@ -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 --
--------------------------
......
......@@ -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;
};
#!/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'))
......@@ -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
......
......@@ -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);
......
This diff is collapsed.
......@@ -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"]
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment