Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Psi matrix #22

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
[ADD] state space solver
rafael committed Aug 16, 2022
commit 846a30e03a4760c2f5523d5ce6aef2bff4683895
25 changes: 13 additions & 12 deletions hdl/psi_fix_matrix_mult.vhd
Original file line number Diff line number Diff line change
@@ -31,23 +31,24 @@ library work;
entity psi_fix_matrix_mult is
generic( RstPol_g : std_logic := '1'; -- Reset polarity

InFmt_g : PsiFixFmt_t := (1, 0, 15); -- Input data format
InAFmt_g : PsiFixFmt_t := (1, 0, 15); -- Input data format
InBFmt_g : PsiFixFmt_t := (1, 0, 15); -- Input data format for matrix B
OutFmt_g : PsiFixFmt_t := (1, 0, 16); -- Output data format
IntFmt_g : PsiFixFmt_t := (1, 1, 30); -- Internal data format

matA_N_g : positive := 2; -- Number of rows on matrix A
matA_M_g : positive := 2; -- Number of columns on matrix A
matA_N_g : positive := 3; -- Number of rows on matrix A
matA_M_g : positive := 3; -- Number of columns on matrix A

matB_N_g : positive := 2; -- Number of rows on matrix B
matB_N_g : positive := 3; -- Number of rows on matrix B
matB_M_g : positive := 2 -- Number of columns on matrix B
);

port( rst_i : in std_logic; -- Input reset
clk_i : in std_logic; -- Input clock
strb_i : in std_logic; -- Input strobe

data_A_i : in std_logic_vector( (matA_N_g*matA_M_g*PsiFixSize(InFmt_g)) - 1 downto 0 ); -- Input data matrix A
data_B_i : in std_logic_vector( (matB_N_g*matB_M_g*PsiFixSize(InFmt_g)) - 1 downto 0 ); -- Input data matrix B
data_A_i : in std_logic_vector( (matA_N_g*matA_M_g*PsiFixSize(InAFmt_g)) - 1 downto 0 ); -- Input data matrix A
data_B_i : in std_logic_vector( (matB_N_g*matB_M_g*PsiFixSize(InBFmt_g)) - 1 downto 0 ); -- Input data matrix B

strb_o : out std_logic; -- Output strobe
data_o : out std_logic_vector( (matA_N_g*matB_M_g*PsiFixSize(OutFmt_g)) - 1 downto 0 ) -- Output data [A * B] matrix
@@ -71,12 +72,12 @@ architecture behavioral of psi_fix_matrix_mult is
-- Registers always present
strb : std_logic_vector(4 downto 0);

matA_s : data_matrix_t(0 to (matA_N_g-1), 0 to (matA_M_g-1)) (PsiFixSize(InFmt_g) - 1 downto 0);
matB_s : data_matrix_t(0 to (matB_N_g-1), 0 to (matB_M_g-1)) (PsiFixSize(InFmt_g) - 1 downto 0);
matA_s : data_matrix_t(0 to (matA_N_g-1), 0 to (matA_M_g-1)) (PsiFixSize(InAFmt_g) - 1 downto 0);
matB_s : data_matrix_t(0 to (matB_N_g-1), 0 to (matB_M_g-1)) (PsiFixSize(InBFmt_g) - 1 downto 0);

mult_s : arry_matrix_t(0 to (matB_M_g-1))(0 to (matA_N_g-1), 0 to (matA_M_g-1)) (PsiFixSize(IntFmt_g) - 1 downto 0);

add_s : data_matrix_t(0 to (matB_N_g-1), 0 to (matB_M_g-1)) (PsiFixSize(IntFmt_g) - 1 downto 0);
add_s : data_matrix_t(0 to (matA_N_g-1), 0 to (matB_M_g-1)) (PsiFixSize(IntFmt_g) - 1 downto 0);

data_s : std_logic_vector( (matA_N_g*matB_M_g*PsiFixSize(OutFmt_g)) - 1 downto 0 );

@@ -106,13 +107,13 @@ begin
------------------------------------------------------------
for i in 0 to (matA_N_g - 1) loop
for j in 0 to (matA_M_g - 1) loop
r_next.matA_s(i, j) <= data_A_i( (((i*matA_M_g)+j+1)*PsiFixSize(InFmt_g))-1 downto (((i*matA_M_g)+j)*PsiFixSize(InFmt_g)) );
r_next.matA_s(i, j) <= data_A_i( (((i*matA_M_g)+j+1)*PsiFixSize(InAFmt_g))-1 downto (((i*matA_M_g)+j)*PsiFixSize(InAFmt_g)) );
end loop;
end loop;

for i in 0 to (matB_N_g - 1) loop
for j in 0 to (matB_M_g - 1) loop
r_next.matB_s(i, j) <= data_B_i( (((i*matB_M_g)+j+1)*PsiFixSize(InFmt_g))-1 downto (((i*matB_M_g)+j)*PsiFixSize(InFmt_g)) );
r_next.matB_s(i, j) <= data_B_i( (((i*matB_M_g)+j+1)*PsiFixSize(InBFmt_g))-1 downto (((i*matB_M_g)+j)*PsiFixSize(InBFmt_g)) );
end loop;
end loop;

@@ -122,7 +123,7 @@ begin
for i in 0 to (matB_M_g - 1) loop
for j in 0 to (matA_N_g - 1) loop
for k in 0 to (matA_M_g - 1) loop
r_next.mult_s(i)(j, k) <= PsiFixMult(r.matA_s(j, k), InFmt_g, r.matB_s(k, i), InFmt_g, IntFmt_g, PsiFixTrunc, PsiFixWrap);
r_next.mult_s(i)(j, k) <= PsiFixMult(r.matA_s(j, k), InAFmt_g, r.matB_s(k, i), InBFmt_g, IntFmt_g, PsiFixTrunc, PsiFixWrap);
end loop;
end loop;
end loop;
226 changes: 226 additions & 0 deletions hdl/psi_fix_ss_solver.vhd
Original file line number Diff line number Diff line change
@@ -0,0 +1,226 @@
----------------------------------------------------------------------------------
-- Copyright (c) 2018 by Paul Scherrer Institute, Switzerland
-- All rights reserved.
-- Authors: Rafael Basso
----------------------------------------------------------------------------------

----------------------------------------------------------------------------------
-- Description:
----------------------------------------------------------------------------------
--
--
----------------------------------------------------------------------------------

----------------------------------------------------------------------------------
-- Libraries
----------------------------------------------------------------------------------

library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;

library work;
use work.psi_fix_pkg.all;
use work.psi_common_math_pkg.all;

----------------------------------------------------------------------------------
-- Entity
----------------------------------------------------------------------------------

entity psi_fix_ss_solver is
generic( RstPol_g : std_logic := '1'; -- Reset polarity

InFmt_g : PsiFixFmt_t := (1, 0, 15); -- Input fix point format
OutFmt_g : PsiFixFmt_t := (1, 29, 2); -- Output fix point formar
IntFmt_g : PsiFixFmt_t := (1, 29, 2); -- Internal fix point format

CoefAFmt_g : PsiFixFmt_t := (1, 0, 31); -- Coefficient matrix A fix point format
CoefBFmt_g : PsiFixFmt_t := (1, 29, 2); -- Coefficient matrix B fix point format

Order_g : positive := 2 -- State space system order
);

port( rst_i : in std_logic; -- Input reset
clk_i : in std_logic; -- Input clock
strb_i : in std_logic; -- Inpput strobe

data_i : in std_logic_vector( Order_g*PsiFixSize(InFmt_g) - 1 downto 0 ); -- Input data

coeff_A_i : in std_logic_vector( ((Order_g**2)*PsiFixSize(CoefAFmt_g)) - 1 downto 0 ); -- Input A matrix coefficients data (an array of [(Order_g^2) - 1] elements with the CoefAFmt_g size)
coeff_B_i : in std_logic_vector( PsiFixSize(CoefBFmt_g) - 1 downto 0 ); -- Input B matrix coefficients data (an array of [(Order_g) - 1] elements with the CoefBFmt_g size)

strb_o : out std_logic; -- Output strobe
data_o : out std_logic_vector( Order_g*PsiFixSize(OutFmt_g) - 1 downto 0 ) -- Output data
);
end psi_fix_ss_solver;

----------------------------------------------------------------------------------
-- Architecture
----------------------------------------------------------------------------------

architecture behavioral of psi_fix_ss_solver is

-- TYPE
---------------

-- Two process method
type two_process_r is record
-- Registers always present
strb_s : std_logic_vector(3 downto 0);

data_Ax_s : std_logic_vector( Order_g*PsiFixSize(IntFmt_g)-1 downto 0);
data_Bu_s : std_logic_vector( Order_g*PsiFixSize(IntFmt_g)-1 downto 0);

data_prev_s : std_logic_vector( Order_g*PsiFixSize(IntFmt_g)-1 downto 0 );

data_out_s : std_logic_vector( Order_g*PsiFixSize(OutFmt_g)-1 downto 0 );
end record;

-- CONSTANT
---------------

constant width_c : positive := Order_g*PsiFixSize(InFmt_g);

-- SIGNAL
---------------
signal r, r_next : two_process_r;

signal strb_A_s : std_logic;
signal strb_B_s : std_logic;

signal data_Ax_s : std_logic_vector( Order_g*PsiFixSize(IntFmt_g) - 1 downto 0);
signal data_Bu_s : std_logic_vector( Order_g*PsiFixSize(IntFmt_g) - 1 downto 0);

signal data_s : std_logic_vector( Order_g*PsiFixSize(InFmt_g) downto 0);

begin

--------------------------------------------------------------------------
-- Combinatorial Process
--------------------------------------------------------------------------

p_comb : process(r, strb_i, strb_A_s, strb_B_s, data_i, data_Ax_s, data_Bu_s)
begin

-- stage 0
------------------------------------------------------------
r_next.strb_s(0) <= strb_A_s and strb_B_s;

r_next.data_Ax_s <= data_Ax_s;
r_next.data_Bu_s <= data_Bu_s;


-- stage 1
------------------------------------------------------------
for i in 0 to (Order_g-1) loop
r_next.data_prev_s((i+1)*PsiFixSize(IntFmt_g)-1 downto i*PsiFixSize(IntFmt_g)) <= PsiFixAdd(r.data_Ax_s((i+1)*PsiFixSize(IntFmt_g)-1 downto i*PsiFixSize(IntFmt_g)), IntFmt_g,
r.data_Bu_s((i+1)*PsiFixSize(IntFmt_g)-1 downto i*PsiFixSize(IntFmt_g)), IntFmt_g,
IntFmt_g, PsiFixTrunc, PsiFixWrap);
end loop;

-- stage 2
------------------------------------------------------------
for i in 0 to (Order_g-1) loop
r_next.data_out_s((i+1)*PsiFixSize(OutFmt_g)-1 downto i*PsiFixSize(OutFmt_g)) <= PsiFixResize(r.data_prev_s((i+1)*PsiFixSize(IntFmt_g)-1 downto i*PsiFixSize(IntFmt_g)), IntFmt_g, OutFmt_g, PsiFixTrunc, PsiFixWrap);
end loop;

r_next.strb_s(r_next.strb_s'high downto 1) <= r.strb_s(r.strb_s'high-1 downto 0);

end process;

--------------------------------------------------------------------------
-- Output Assignment
--------------------------------------------------------------------------

strb_o <= r.strb_s(2);
data_o <= r.data_out_s;

--------------------------------------------------------------------------
-- Sequential Process
--------------------------------------------------------------------------

p_seq : process(rst_i, clk_i)
begin
if(rising_edge(clk_i)) then
if(rst_i = RstPol_g) then
r.strb_s <= (others => '0');

r.data_Ax_s <= (others => '0');
r.data_Bu_s <= (others => '0');

r.data_prev_s <= (others => '0');

r.data_out_s <= (others => '0');
else
if(r_next.strb_s /= "0000") then
r <= r_next;
end if;
end if;
end if;
end process;

--------------------------------------------------------------------------
-- A Matrix Multiplier instantiation
--------------------------------------------------------------------------

u_mulA : entity work.psi_fix_matrix_mult
generic map(
RstPol_g => RstPol_g,

InAFmt_g => CoefAFmt_g,
InBFmt_g => IntFmt_g,

OutFmt_g => IntFmt_g,
IntFmt_g => IntFmt_g,

matA_N_g => Order_g,
matA_M_g => Order_g,

matB_N_g => Order_g,
matB_M_g => 1
)
port map(
rst_i => rst_i,
clk_i => clk_i,
strb_i => strb_i,

data_A_i => coeff_A_i,
data_B_i => r.data_prev_s,

strb_o => strb_A_s,
data_o => data_Ax_s
);

--------------------------------------------------------------------------
-- B Matrix Multiplier instantiation
--------------------------------------------------------------------------

u_mulB : entity work.psi_fix_matrix_mult
generic map(
RstPol_g => RstPol_g,

InAFmt_g => InFmt_g,
InBFmt_g => CoefBFmt_g,

OutFmt_g => IntFmt_g,
IntFmt_g => IntFmt_g,

matA_N_g => Order_g,
matA_M_g => 1,

matB_N_g => 1,
matB_M_g => 1
)
port map(
rst_i => rst_i,
clk_i => clk_i,
strb_i => strb_i,

data_A_i => data_i,
data_B_i => coeff_B_i,

strb_o => strb_B_s,
data_o => data_Bu_s
);

end behavioral;
64 changes: 64 additions & 0 deletions model/psi_fix_matrix_mult.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
########################################################################################################################
# Copyright (c) 2018 by Paul Scherrer Institute, Switzerland
# All rights reserved.
# Authors: Rafael Basso
########################################################################################################################

########################################################################################################################
## Imports
########################################################################################################################

from psi_fix_pkg import *

########################################################################################################################
## psi_fix_matrix_mult class
########################################################################################################################

class psi_fix_matrix_mult:

def __init__(self, InAFmt : PsiFixFmt, InBFmt : PsiFixFmt, OutFmt : PsiFixFmt, IntFmt : PsiFixFmt, matA_row : int, matA_col: int, matB_row: int, matB_col: int):
"""
Constructor for the psi fix matrix multilier object
:param InAFmt : Input fix point format for matrix A
:param InBFmt : Input fix point format for matrix B
:param OutFmt : Output fix point format
:param IntFmt : Internal fix point format
:param matA_row : Number of rows on matrix A
:param matA_col : Number of columns on matrix A
:param matB_row : Number of rows on matrix B
:param matB_col : Number of columns on matrix B
"""
self.InAFmt = InAFmt
self.InBFmt = InBFmt
self.OutFmt = OutFmt
self.IntFmt = IntFmt

assert matA_col == matB_row, "###ERROR###: Matrices have incompatible dimensions"

self.matA_row = matA_row
self.matA_col = matA_col

self.matB_row = matB_row
self.matB_col = matB_col

def process(self, matA, matB):
"""
Matrices multiplication process
C = A x B
:param matA : Matrix A (matA_row x matA_col)
:param matB : Matrix B (matB_row x matB_col)
:return matC : Matrix C, result of A x B
"""
matC = []

for i in range(0, self.matA_row):
for j in range(0, self.matB_col):

add = PsiFixFromReal(0, self.IntFmt)
for k in range(0, self.matB_row):
mult = PsiFixMult(matA[(i*self.matA_col)+k], self.InAFmt, matB[(k*self.matB_col)+j], self.InBFmt, self.IntFmt, PsiFixRnd.Trunc, PsiFixSat.Wrap)
add = PsiFixAdd(add, self.IntFmt, mult, self.IntFmt, self.IntFmt, PsiFixRnd.Trunc, PsiFixSat.Wrap)

matC.append(PsiFixResize(add, self.IntFmt, self.OutFmt, PsiFixRnd.Trunc, PsiFixSat.Wrap))

return matC
Loading