greek64/rtps-fpga
2
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add Fibonacci Action Feedback Topic

Browse Source
This commit is contained in:
John Ring 2022-02-01 15:26:58 +01:00
parent 45556ab136
commit dc47f72942
7 changed files with 1580 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
sim/L1_Fibonacci_action_feedback_test1.do Normal file
View File

@ -0,0 +1,58 @@
onerror {resume}
quietly WaveActivateNextPane {} 0
add wave -noupdate -divider SYSTEM
add wave -noupdate /l1_fibonacci_action_feedback_test1/clk
add wave -noupdate /l1_fibonacci_action_feedback_test1/reset
add wave -noupdate -divider PUBLISHER
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/start_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/ack_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/opcode_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/done_dds
add wave -noupdate -radix unsigned /l1_fibonacci_action_feedback_test1/uut_pub/return_code_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/start_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/opcode_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/ack_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/done_user
add wave -noupdate -radix unsigned /l1_fibonacci_action_feedback_test1/uut_pub/return_code_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/stage
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/encode_stage
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/cnt
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/goal_id_cnt
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_pub/seq_cnt
add wave -noupdate -divider SUBSCRIBER
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/start_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/ack_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/opcode_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/get_data_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/done_dds
add wave -noupdate -radix unsigned /l1_fibonacci_action_feedback_test1/uut_sub/return_code_dds
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/start_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/opcode_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/ack_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/done_user
add wave -noupdate -radix unsigned /l1_fibonacci_action_feedback_test1/uut_sub/return_code_user
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/stage
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/decode_stage
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/return_stage
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/cnt
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/goal_id_cnt
add wave -noupdate /l1_fibonacci_action_feedback_test1/uut_sub/seq_cnt
add wave -noupdate -divider MISC
TreeUpdate [SetDefaultTree]
WaveRestoreCursors {{Cursor 1} {8403681 ps} 0}
quietly wave cursor active 1
configure wave -namecolwidth 150
configure wave -valuecolwidth 100
configure wave -justifyvalue left
configure wave -signalnamewidth 1
configure wave -snapdistance 10
configure wave -datasetprefix 0
configure wave -rowmargin 4
configure wave -childrowmargin 2
configure wave -gridoffset 0
configure wave -gridperiod 1
configure wave -griddelta 40
configure wave -timeline 0
configure wave -timelineunits ns
update
WaveRestoreZoom {7833600 ps} {8857600 ps}

370
src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd Normal file
View File

@ -0,0 +1,370 @@
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library osvvm; -- Utility Library
context osvvm.OsvvmContext;
use work.rtps_package.all;
use work.ros_package.all;
use work.rtps_test_package.all;
use work.Fibonacci_package.all;
-- This testbench tests the General Behavour of ROS publishers and subscribers
-- More specifically following tests are done:
-- * Test Unssuported Opcode Operations
-- * Test RETCODE_NO_DATA response from DDS Reader
-- * Test RETCODE_ERROR response from DDS Reader
-- * Test RETCODE_ERROR response from DDS Writer
-- * Test Sample with No Valid response from DDS Reader
-- * Test Big Endian Encoding/Decoding of Messages
entity L1_Fibonacci_action_feedback_test1 is
end entity;
architecture testbench of L1_Fibonacci_action_feedback_test1 is
signal clk, reset : std_logic := '0';
signal valid, ready, last_word, last_word_out_w : std_logic := '0';
signal data : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
signal start_sub, start_pub, ack_sub, ack_pub, taken_sub, taken_pub, done_sub, done_pub : std_logic := '0';
signal opcode_sub, opcode_pub : ROS_TOPIC_OPCODE_TYPE := NOP;
signal return_code_sub, return_code_pub : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
signal start_r, si_valid_data_r : std_logic := '0';
signal return_code_r, return_code_w : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := (others => '0');
signal ready_w, selector : std_logic := '0';
signal message_info_sub : MESSAGE_INFO_TYPE := EMPTY_MESSAGE_INFO;
--
signal goal_id_addr_sub, goal_id_addr_pub : std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0) := (others => '0');
signal goal_id_ready_sub, goal_id_ready_pub, goal_id_ren_sub, goal_id_ren_pub, goal_id_wen_pub, goal_id_valid_sub, goal_id_valid_pub, goal_id_ack_sub, goal_id_ack_pub : std_logic := '0';
signal goal_id_sub, goal_id_r_pub, goal_id_w_pub : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
signal seq_len_sub, seq_len_pub, seq_addr_sub, seq_addr_pub : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
signal seq_ready_sub, seq_ready_pub, seq_ren_sub, seq_ren_pub, seq_wen_pub, seq_valid_sub, seq_valid_pub, seq_ack_sub, seq_ack_pub : std_logic := '0';
signal seq_sub, seq_r_pub, seq_w_pub : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
begin
uut_sub : entity work.Fibonacci_action_feedback_sub(arch)
port map (
clk => clk,
reset => reset,
start_dds => start_r,--
ack_dds => '1',--
opcode_dds => open,
instance_state_dds => open,
view_state_dds => open,
sample_state_dds => open,
instance_handle_dds => open,
max_samples_dds => open,
get_data_dds => open,--
done_dds => '1',--
return_code_dds => return_code_r,--
valid_in_dds => valid,--
ready_in_dds => ready,--
data_in_dds => data,--
last_word_in_dds => last_word,--
si_sample_state_dds => ANY_SAMPLE_STATE,
si_view_state_dds => ANY_VIEW_STATE,
si_instance_state_dds => ANY_INSTANCE_STATE,
si_source_timestamp_dds => TIME_INVALID,
si_instance_handle_dds => HANDLE_NIL,
si_publication_handle_dds => HANDLE_NIL,
si_disposed_generation_count_dds => (others => '0'),
si_no_writers_generation_count_dds => (others => '0'),
si_sample_rank_dds => (others => '0'),
si_generation_rank_dds => (others => '0'),
si_absolute_generation_rank_dds => (others => '0'),
si_valid_data_dds => si_valid_data_r,--
si_valid_dds => '1',--
si_ack_dds => open,--
eoc_dds => '1',--
status_dds => (others => '0'),
start_user => start_sub,
opcode_user => opcode_sub,
ack_user => ack_sub,
done_user => done_sub,
return_code_user => return_code_sub,
data_available_user => open,
goal_id_addr => goal_id_addr_sub,
goal_id_ready => goal_id_ready_sub,
goal_id_ren => goal_id_ren_sub,
goal_id_valid => goal_id_valid_sub,
goal_id_ack => goal_id_ack_sub,
goal_id => goal_id_sub,
seq_len => seq_len_sub,
seq_addr => seq_addr_sub,
seq_ready => seq_ready_sub,
seq_ren => seq_ren_sub,
seq_valid => seq_valid_sub,
seq_ack => seq_ack_sub,
seq => seq_sub,
message_info_user => message_info_sub,
taken_user => taken_sub
);
uut_pub : entity work.Fibonacci_action_feedback_pub(arch)
port map (
clk => clk,
reset => reset,
start_dds => open,--
ack_dds => '1',--
opcode_dds => open,
instance_handle_out_dds => open,
source_ts_dds => open,
max_wait_dds => open,
done_dds => '1',--
return_code_dds => return_code_w,--
instance_handle_in_dds => HANDLE_NIL,
valid_out_dds => valid,--
ready_out_dds => ready_w,--ready,--
data_out_dds => data,--
last_word_out_dds => last_word_out_w,--last_word,--
valid_in_dds => '0',
ready_in_dds => open,
data_in_dds => (others => '0'),
last_word_in_dds => '0',
status_dds => (others => '0'),
start_user => start_pub,--
ack_user => ack_pub,--
opcode_user => opcode_pub,--
goal_id_addr => goal_id_addr_pub,
goal_id_ready => goal_id_ready_pub,
goal_id_ren => goal_id_ren_pub,
goal_id_wen => goal_id_wen_pub,
goal_id_valid => goal_id_valid_pub,
goal_id_ack => goal_id_ack_pub,
goal_id_r => goal_id_r_pub,
goal_id_w => goal_id_w_pub,
seq_len => seq_len_pub,
seq_addr => seq_addr_pub,
seq_ready => seq_ready_pub,
seq_ren => seq_ren_pub,
seq_wen => seq_wen_pub,
seq_valid => seq_valid_pub,
seq_ack => seq_ack_pub,
seq_r => seq_r_pub,
seq_w => seq_w_pub,
done_user => done_pub,--
return_code_user => return_code_pub --
);
process (all)
begin
if (selector = '1') then
ready_w <= '1';
last_word <= '0';
else
ready_w <= ready;
last_word <= last_word_out_w;
end if;
end process;
stimulus_prc : process
variable RV : RandomPType;
variable GOAL_ID, SEQ : AlertLogIDType;
procedure wait_on_sig(signal sig : std_logic) is
begin
if (sig /= '1') then
wait on sig until sig = '1';
end if;
end procedure;
begin
SetAlertLogName("AddTwoInts Service - Level 1 - (Big Endian) - General");
SetAlertEnable(FAILURE, TRUE);
SetAlertEnable(ERROR, TRUE);
SetAlertEnable(WARNING, TRUE);
SetLogEnable(DEBUG, FALSE);
SetLogEnable(PASSED, FALSE);
SetLogEnable(INFO, TRUE);
RV.InitSeed(RV'instance_name);
GOAL_ID := GetAlertLogID("GOAL_ID", ALERTLOG_BASE_ID);
SEQ := GetAlertLogID("SEQ", ALERTLOG_BASE_ID);
Log("Initial Reset", INFO);
selector <= '1';
return_code_r <= RETCODE_OK;
return_code_w <= RETCODE_OK;
si_valid_data_r <= '0';
start_sub <= '0';
start_pub <= '0';
reset <= '1';
wait until rising_edge(clk);
wait until rising_edge(clk);
reset <= '0';
Log("SUBSCRIBER: Test Unsupported Opcode", INFO);
start_sub <= '1';
opcode_sub <= PUBLISH;
wait_on_sig(ack_sub);
wait until rising_edge(clk);
start_sub <= '0';
wait_on_sig(done_sub);
wait for 1 ps; -- Make sure all signals stable
AlertIf(return_code_sub /= ROS_RET_UNSUPPORTED, "Unexpected Subscriber Response", FAILURE);
wait until rising_edge(clk);
Log("PUBLISHER: Test Unsupported Opcode", INFO);
start_pub <= '1';
opcode_pub <= TAKE;
wait_on_sig(ack_pub);
wait until rising_edge(clk);
start_pub <= '0';
wait_on_sig(done_pub);
wait for 1 ps; -- Make sure all signals stable
AlertIf(return_code_pub /= ROS_RET_UNSUPPORTED, "Unexpected Publisher Response", FAILURE);
wait until rising_edge(clk);
Log("SUBSCRIBER: Test No Data", INFO);
return_code_r <= RETCODE_NO_DATA;
start_sub <= '1';
opcode_sub <= TAKE;
wait_on_sig(ack_sub);
wait until rising_edge(clk);
start_sub <= '0';
wait_on_sig(done_sub);
wait for 1 ps; -- Make sure all signals stable
AlertIf(return_code_sub /= ROS_RET_OK, "Unexpected Subscriber Response", FAILURE);
AlertIf(taken_sub /= '0', "Subscriber taken is unexpectedly set", FAILURE);
wait until rising_edge(clk);
Log("SUBSCRIBER: Test Reader Error", INFO);
return_code_r <= RETCODE_ERROR;
start_sub <= '1';
opcode_sub <= TAKE;
wait_on_sig(ack_sub);
wait until rising_edge(clk);
start_sub <= '0';
wait_on_sig(done_sub);
wait for 1 ps; -- Make sure all signals stable
AlertIf(return_code_sub /= ROS_RET_ERROR, "Unexpected Subscriber Response", FAILURE);
wait until rising_edge(clk);
Log("PUBLISHER: Test Writer Error", INFO);
return_code_w <= RETCODE_ERROR;
start_pub <= '1';
opcode_pub <= PUBLISH;
wait_on_sig(ack_pub);
wait until rising_edge(clk);
start_pub <= '0';
wait_on_sig(done_pub);
wait for 1 ps; -- Make sure all signals stable
AlertIf(return_code_pub /= ROS_RET_ERROR, "Unexpected Publisher Response", FAILURE);
wait until rising_edge(clk);
return_code_r <= RETCODE_OK;
return_code_w <= RETCODE_OK;
selector <= '0';
Log("Setting Setting Data in Publisher Side", INFO);
for i in 0 to GOAL_ID_MAX_DEPTH-1 loop
goal_id_addr_pub <= int(i,goal_id_addr_pub'length);
goal_id_w_pub <= RV.RandSlv(goal_id_w_pub'length);
wait_on_sig(goal_id_ready_pub);
goal_id_wen_pub <= '1';
wait for TEST_CLOCK_PERIOD;
goal_id_wen_pub <= '0';
end loop;
for i in 0 to 9 loop
seq_len_pub <= int(10,seq_len_pub'length);
seq_addr_pub <= int(i,seq_addr_pub'length);
seq_w_pub <= RV.RandSlv(seq_w_pub'length);
wait_on_sig(seq_ready_pub);
seq_wen_pub <= '1';
wait for TEST_CLOCK_PERIOD;
seq_wen_pub <= '0';
end loop;
Log("Publish message", INFO);
start_pub <= '1';
opcode_pub <= PUBLISH;
wait_on_sig(ack_pub);
wait until rising_edge(clk);
start_pub <= '0';
Log("Take message", INFO);
start_sub <= '1';
opcode_sub <= TAKE;
wait_on_sig(ack_sub);
wait until rising_edge(clk);
start_sub <= '0';
-- TEST NO VALID DATA
wait_on_sig(start_r);
wait until rising_edge(clk);
wait until rising_edge(clk);
wait_on_sig(start_r);
si_valid_data_r <= '1';
Log("Wait for Data", INFO);
wait_on_sig(done_sub);
wait until rising_edge(clk);
AlertIf(return_code_sub /= ROS_RET_OK, "Subscriber did Return ERROR", FAILURE);
AlertIf(taken_sub /= '1', "Subscriber did not take Message", FAILURE);
Log("Compare Messages", INFO);
for i in 0 to GOAL_ID_MAX_DEPTH-1 loop
goal_id_addr_pub <= int(i,goal_id_addr_pub'length);
goal_id_addr_sub <= int(i,goal_id_addr_sub'length);
wait_on_sig(goal_id_ready_pub);
wait_on_sig(goal_id_ready_sub);
goal_id_ren_pub <= '1';
goal_id_ren_sub <= '1';
wait for TEST_CLOCK_PERIOD;
goal_id_ren_pub <= '0';
goal_id_ren_sub <= '0';
wait_on_sig(goal_id_valid_pub);
wait_on_sig(goal_id_valid_sub);
AffirmIfEqual(SEQ, goal_id_sub, goal_id_r_pub);
goal_id_ack_pub <= '1';
goal_id_ack_sub <= '1';
wait for TEST_CLOCK_PERIOD;
goal_id_ack_pub <= '0';
goal_id_ack_sub <= '0';
end loop;
AffirmIfEqual(seq_len_sub, seq_len_pub);
for i in 0 to to_integer(unsigned(seq_len_pub)) loop
seq_addr_pub <= int(i,seq_addr_pub'length);
seq_addr_sub <= int(i,seq_addr_sub'length);
wait_on_sig(seq_ready_pub);
wait_on_sig(seq_ready_sub);
seq_ren_pub <= '1';
seq_ren_sub <= '1';
wait for TEST_CLOCK_PERIOD;
seq_ren_pub <= '0';
seq_ren_sub <= '0';
wait_on_sig(seq_valid_pub);
wait_on_sig(seq_valid_sub);
AffirmIfEqual(SEQ, seq_sub, seq_r_pub);
seq_ack_pub <= '1';
seq_ack_sub <= '1';
wait for TEST_CLOCK_PERIOD;
seq_ack_pub <= '0';
seq_ack_sub <= '0';
end loop;
TranscriptOpen(RESULTS_FILE, APPEND_MODE);
SetTranscriptMirror;
ReportAlerts;
TranscriptClose;
std.env.stop;
wait;
end process;
clock_prc : process
begin
clk <= '0';
wait for TEST_CLOCK_PERIOD/2;
clk <= '1';
wait for TEST_CLOCK_PERIOD/2;
end process;
watchdog : process
begin
wait for 1 ms;
Alert("Test timeout", FAILURE);
std.env.stop;
end process;
end architecture;

10
src/ros2/Tests/ros_testbench.pro
View File

@ -83,13 +83,23 @@ analyze ../../TEMPLATE_user_config.vhd
analyze ../../rtps_config_package.vhd analyze ../../rtps_config_package.vhd
analyze ../../rtps_test_package.vhd analyze ../../rtps_test_package.vhd
analyze ../ros_package.vhd analyze ../ros_package.vhd
analyze ../../single_port_ram.vhd
analyze ../../Tests/single_port_ram_cfg.vhd
analyze ../../FWFT_FIFO.vhd
analyze ../../Tests/FWFT_FIFO_cfg.vhd
analyze ../../mem_ctrl.vhd
analyze ../example_interfaces/AddTwoInts_srv_client.vhd analyze ../example_interfaces/AddTwoInts_srv_client.vhd
analyze ../example_interfaces/AddTwoInts_srv_server.vhd analyze ../example_interfaces/AddTwoInts_srv_server.vhd
analyze Level_1/L1_AddTwoInts_srv_test1.vhd analyze Level_1/L1_AddTwoInts_srv_test1.vhd
analyze Level_1/L1_AddTwoInts_srv_test2.vhd analyze Level_1/L1_AddTwoInts_srv_test2.vhd
analyze Level_2/L2_AddTwoInts_test1.vhd analyze Level_2/L2_AddTwoInts_test1.vhd
analyze ../example_interfaces/Fibonacci_package.vhd
analyze ../example_interfaces/Fibonacci_action_feedback_pub.vhd
analyze ../example_interfaces/Fibonacci_action_feedback_sub.vhd
analyze Level_1/L1_Fibonacci_action_feedback_test1.vhd
simulate L1_AddTwoInts_srv_test1 simulate L1_AddTwoInts_srv_test1
simulate L1_AddTwoInts_srv_test2 simulate L1_AddTwoInts_srv_test2
simulate L2_AddTwoInts_test1 simulate L2_AddTwoInts_test1
simulate L1_Fibonacci_action_feedback_test1

451
src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd Normal file
View File

@ -0,0 +1,451 @@
-- altera vhdl_input_version vhdl_2008
-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html)
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.rtps_package.all;
use work.rtps_config_package.all;
use work.ros_package.all;
use work.Fibonacci_package.all;
entity Fibonacci_action_feedback_pub is
generic (
LITTLE_ENDIAN : std_logic := '0'
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
-- FROM DDS WRITER
start_dds : out std_logic;
ack_dds : in std_logic;
opcode_dds : out DDS_WRITER_OPCODE_TYPE;
instance_handle_out_dds : out INSTANCE_HANDLE_TYPE;
source_ts_dds : out TIME_TYPE;
max_wait_dds : out DURATION_TYPE;
done_dds : in std_logic;
return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
instance_handle_in_dds : in INSTANCE_HANDLE_TYPE;
valid_out_dds : out std_logic;
ready_out_dds : in std_logic;
data_out_dds : out std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_out_dds : out std_logic;
valid_in_dds : in std_logic;
ready_in_dds : out std_logic;
data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_dds : in std_logic;
-- Communication Status
status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- TO USER ENTITY
start_user : in std_logic;
opcode_user : in ROS_TOPIC_OPCODE_TYPE;
ack_user : out std_logic;
-- ###GENERATED START###
goal_id_addr : in std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0);
goal_id_ready : out std_logic;
goal_id_ren : in std_logic;
goal_id_wen : in std_logic;
goal_id_valid : out std_logic;
goal_id_ack : in std_logic;
goal_id_r : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
goal_id_w : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
seq_len : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0);
seq_addr : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0);
seq_ready : out std_logic;
seq_ren : in std_logic;
seq_wen : in std_logic;
seq_valid : out std_logic;
seq_ack : in std_logic;
seq_r : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
seq_w : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
-- ###GENERATED END###
done_user : out std_logic;
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)
);
end entity;
architecture arch of Fibonacci_action_feedback_pub is
--*****TYPE DECLARATION*****
-- FSM states. Explained below in detail
type STAGE_TYPE is (IDLE,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_STREAM,ENCODE_PAYLOAD,WAIT_FOR_WRITER,RETURN_ROS);
-- ###GENERATED START###
type ENCODE_STAGE_TYPE is (WRITE_GOAL_ID, WRITE_SEQ_LENGTH, WRITE_SEQ);
-- ###GENERATED END###
-- *MAIN PROCESS*
signal stage, stage_next : STAGE_TYPE;
signal cnt, cnt_next : natural range 0 to 5;
signal align_offset, align_offset_next : unsigned(MAX_ALIGN_OFFSET_WIDTH-1 downto 0);
signal align_op, align_op_next : std_logic;
signal target_align, target_align_next : ALIGN_TYPE;
signal data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
signal abort_mem : std_logic;
signal finalize_payload, finalize_payload_next : std_logic;
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE;
-- ###GENERATED START###
signal goal_id_cnt, goal_id_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1;
signal seq_cnt, seq_cnt_next : natural range 0 to SEQ_MAX_DEPTH-1;
-- goal_id_mem SIGNALS
signal goal_id_mem_addr : std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0);
signal goal_id_mem_read, goal_id_mem_ready_in, goal_id_mem_ready_out, goal_id_mem_valid_in, goal_id_mem_valid_out : std_logic;
signal goal_id_mem_data_in, goal_id_mem_data_out : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- seq_mem SIGNALS
signal seq_mem_addr : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0);
signal seq_mem_read, seq_mem_ready_in, seq_mem_ready_out, seq_mem_valid_in, seq_mem_valid_out : std_logic;
signal seq_mem_data_in, seq_mem_data_out : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
-- ###GENERATED END###
begin
-- ###GENERATED START###
goal_id_mem : entity work.mem_ctrl(arch)
generic map (
ADDR_WIDTH => GOAL_ID_ADDR_WIDTH,
DATA_WIDTH => CDR_INT8_WIDTH,
MEMORY_DEPTH => GOAL_ID_MAX_DEPTH,
MAX_BURST_LENGTH => 1
)
port map (
clk => clk,
reset => reset or abort_mem,
addr => goal_id_mem_addr,
read => goal_id_mem_read,
ready_in => goal_id_mem_ready_in,
valid_in => goal_id_mem_valid_in,
data_in => goal_id_mem_data_in,
ready_out => goal_id_mem_ready_out,
valid_out => goal_id_mem_valid_out,
data_out => goal_id_mem_data_out
);
seq_mem : entity work.mem_ctrl(arch)
generic map (
ADDR_WIDTH => SEQ_ADDR_WIDTH,
DATA_WIDTH => CDR_LONG_WIDTH,
MEMORY_DEPTH => SEQ_MAX_DEPTH,
MAX_BURST_LENGTH => 1
)
port map (
clk => clk,
reset => reset or abort_mem,
addr => seq_mem_addr,
read => seq_mem_read,
ready_in => seq_mem_ready_in,
valid_in => seq_mem_valid_in,
data_in => seq_mem_data_in,
ready_out => seq_mem_ready_out,
valid_out => seq_mem_valid_out,
data_out => seq_mem_data_out
);
-- ###GENERATED END###
-- PASSTHROUGH
instance_handle_out_dds <= HANDLE_NIL;
source_ts_dds <= TIME_INVALID;
max_wait_dds <= DURATION_ZERO;
ready_in_dds <= '0'; -- DDS Writer Input is unused
-- ###GENERATED START###
goal_id_valid <= goal_id_mem_valid_out;
goal_id_r <= goal_id_mem_data_out;
goal_id_mem_data_in <= goal_id_w;
seq_valid <= seq_mem_valid_out;
seq_r <= seq_mem_data_out;
seq_mem_data_in <= seq_w;
-- ###GENERATED END###
main_prc : process (all)
begin
-- DEFAULT
stage_next <= stage;
encode_stage_next <= encode_stage;
cnt_next <= cnt;
align_offset_next <= align_offset;
align_op_next <= align_op;
target_align_next <= target_align;
data_out_latch_next <= data_out_latch;
finalize_payload_next <= finalize_payload;
return_code_latch_next <= return_code_latch;
abort_mem <= '0';
start_dds <= '0';
opcode_dds <= NOP;
valid_out_dds <= '0';
last_word_out_dds <= '0';
ack_user <= '0';
done_user <= '0';
return_code_user <= ROS_RET_OK;
data_out_dds <= (others => '0');
-- ###GENERATED START###
goal_id_cnt_next <= goal_id_cnt;
goal_id_mem_addr <= (others => '0');
goal_id_mem_read <= '0';
goal_id_mem_valid_in <= '0';
goal_id_mem_ready_out <= '0';
goal_id_ready <= '0';
seq_cnt_next <= seq_cnt;
seq_mem_addr <= (others => '0');
seq_mem_read <= '0';
seq_mem_valid_in <= '0';
seq_mem_ready_out <= '0';
seq_ready <= '0';
-- ###GENERATED END###
case (stage) is
when IDLE =>
if (start_user = '1') then
ack_user <= '1';
case (opcode_user) is
when PUBLISH =>
stage_next <= INITIATE_WRITE;
when others =>
return_code_latch_next <= ROS_RET_UNSUPPORTED;
stage_next <= RETURN_ROS;
end case;
-- RESET
abort_mem <= '1';
else
-- ###GENERATED START###
goal_id_ready <= goal_id_mem_ready_in;
goal_id_mem_addr <= goal_id_addr;
goal_id_mem_read <= goal_id_ren;
goal_id_mem_valid_in <= goal_id_ren or goal_id_wen;
goal_id_mem_ready_out <= goal_id_ack;
seq_ready <= seq_mem_ready_in;
seq_mem_addr <= seq_addr;
seq_mem_read <= seq_ren;
seq_mem_valid_in <= seq_ren or seq_wen;
seq_mem_ready_out <= seq_ack;
-- ###GENERATED END###
end if;
when RETURN_ROS =>
done_user <= '1';
return_code_user <= return_code_latch;
-- DONE
stage_next <= IDLE;
when INITIATE_WRITE =>
start_dds <= '1';
opcode_dds <= WRITE;
if (ack_dds = '1') then
stage_next <= WRITE_PAYLOAD_HEADER;
end if;
when WRITE_PAYLOAD_HEADER =>
valid_out_dds <= '1';
if (LITTLE_ENDIAN = '0') then
data_out_dds <= CDR_BE & x"0000";
else
data_out_dds <= CDR_LE & x"0000";
end if;
-- Output Guard
if (ready_out_dds = '1') then
stage_next <= ENCODE_PAYLOAD;
-- Reset
align_offset_next <= (others => '0');
data_out_latch_next <= (others => '0');
-- ###GENERATED START###
encode_stage_next <= WRITE_GOAL_ID;
goal_id_cnt_next <= 0;
-- ###GENERATED END###
end if;
when PUSH =>
-- Mark Last Word
if (finalize_payload = '1') then
last_word_out_dds <= '1';
end if;
valid_out_dds <= '1';
data_out_dds <= data_out_latch;
-- Output Guard
if (ready_out_dds = '1') then
-- NOTE: Ensures all padding is zero.
data_out_latch_next <= (others => '0');
-- Alignment Operation in process
if (align_op = '1') then
stage_next <= ALIGN_STREAM;
-- Reset
align_op_next <= '0';
-- DONE
elsif (finalize_payload = '1') then
finalize_payload_next <= '0';
stage_next <= WAIT_FOR_WRITER;
else
stage_next <= ENCODE_PAYLOAD;
end if;
end if;
when ALIGN_STREAM =>
-- Target Stream Alignment reached
if (check_align(align_offset, target_align)) then
-- DONE
stage_next <= ENCODE_PAYLOAD;
else
align_offset_next <= align_offset + 1;
-- Need to push Word
if (align_offset(1 downto 0) = "11") then
align_op_next <= '1';
stage_next <= PUSH;
end if;
end if;
when ENCODE_PAYLOAD =>
case (encode_stage) is
-- ###GENERATED START###
when WRITE_GOAL_ID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_STREAM;
else
case (cnt) is
-- GET
when 0 =>
goal_id_mem_addr <= std_logic_vector(to_unsigned(goal_id_cnt,GOAL_ID_ADDR_WIDTH));
goal_id_mem_valid_in <= '1';
goal_id_mem_read <= '1';
-- Memory Operation Guard
if (goal_id_mem_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- WRITE
when 1 =>
goal_id_mem_ready_out <= '1';
-- Memory Operation Guard
if (goal_id_mem_valid_out = '1') then
data_out_latch_next <= write_sub_vector(data_out_latch, endian_swap(LITTLE_ENDIAN, goal_id_mem_data_out), to_integer(align_offset(1 downto 0)), TRUE);
align_offset_next <= align_offset + 1;
cnt_next <= 0;
if (goal_id_cnt = GOAL_ID_MAX_DEPTH-1) then
encode_stage_next <= WRITE_SEQ_LENGTH;
else
goal_id_cnt_next <= goal_id_cnt + 1;
end if;
-- Need to fetch next Word
if(align_offset(1 downto 0) = "11") then
stage_next <= PUSH;
end if;
end if;
when others =>
end case;
end if;
when WRITE_SEQ_LENGTH =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_STREAM;
else
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, std_logic_vector(resize(unsigned(seq_len),WORD_WIDTH)));
align_offset_next <= align_offset + 4;
stage_next <= PUSH;
-- Empty Sequence
if (unsigned(seq_len) = 0) then
-- DONE
stage_next <= PUSH;
finalize_payload_next <= '1';
else
encode_stage_next <= WRITE_SEQ;
cnt_next <= 0;
seq_cnt_next <= 0;
end if;
end if;
when WRITE_SEQ =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_STREAM;
else
case (cnt) is
-- GET
when 0 =>
seq_mem_addr <= std_logic_vector(to_unsigned(seq_cnt,SEQ_ADDR_WIDTH));
seq_mem_valid_in <= '1';
seq_mem_read <= '1';
-- Memory Operation Guard
if (seq_mem_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- WRITE
when 1 =>
seq_mem_ready_out <= '1';
-- Memory Operation Guard
if (seq_mem_valid_out = '1') then
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, seq_mem_data_out);
align_offset_next <= align_offset + 4;
stage_next <= PUSH;
cnt_next <= 0;
-- All Elements processed
if (seq_cnt = unsigned(seq_len)-1) then
-- DONE
stage_next <= PUSH;
finalize_payload_next <= '1';
else
seq_cnt_next <= seq_cnt + 1;
end if;
end if;
when others =>
end case;
end if;
-- ###GENERATED END###
when others =>
null;
end case;
when WAIT_FOR_WRITER =>
if (done_dds = '1') then
case (return_code_dds) is
when RETCODE_OK =>
return_code_latch_next <= ROS_RET_OK;
stage_next <= RETURN_ROS;
when others =>
return_code_latch_next <= ROS_RET_ERROR;
stage_next <= RETURN_ROS;
end case;
end if;
end case;
end process;
sync_prc : process(clk)
begin
if rising_edge(clk) then
if (reset = '1') then
stage <= IDLE;
encode_stage <= WRITE_GOAL_ID;
target_align <= ALIGN_1;
return_code_latch <= ROS_RET_OK;
cnt <= 0;
finalize_payload <= '0';
align_op <= '0';
align_offset <= (others => '0');
data_out_latch <= (others => '0');
-- ###GENERATED START###
goal_id_cnt <= 0;
seq_cnt <= 0;
-- ###GENERATED END###
else
stage <= stage_next;
encode_stage <= encode_stage_next;
target_align <= target_align_next;
return_code_latch <= return_code_latch_next;
cnt <= cnt_next;
finalize_payload <= finalize_payload_next;
align_op <= align_op_next;
align_offset <= align_offset_next;
data_out_latch <= data_out_latch_next;
-- ###GENERATED START###
goal_id_cnt <= goal_id_cnt_next;
seq_cnt <= seq_cnt_next;
-- ###GENERATED END###
end if;
end if;
end process;
end architecture;

593
src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd Normal file
View File

@ -0,0 +1,593 @@
-- altera vhdl_input_version vhdl_2008
-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html)
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.rtps_package.all;
use work.rtps_config_package.all;
use work.ros_package.all;
use work.Fibonacci_package.all;
entity Fibonacci_action_feedback_sub is
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
-- FROM DDS READER
start_dds : out std_logic;
ack_dds : in std_logic;
opcode_dds : out DDS_READER_OPCODE_TYPE;
instance_state_dds : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
view_state_dds : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
sample_state_dds : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
instance_handle_dds : out INSTANCE_HANDLE_TYPE;
max_samples_dds : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
get_data_dds : out std_logic;
done_dds : in std_logic;
return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
valid_in_dds : in std_logic;
ready_in_dds : out std_logic;
data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_dds : in std_logic;
-- Sample Info
si_sample_state_dds : in std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
si_view_state_dds : in std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
si_instance_state_dds : in std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
si_source_timestamp_dds : in TIME_TYPE;
si_instance_handle_dds : in INSTANCE_HANDLE_TYPE;
si_publication_handle_dds : in INSTANCE_HANDLE_TYPE;
si_disposed_generation_count_dds : in std_logic_vector(DISPOSED_GENERATION_COUNT_WIDTH-1 downto 0);
si_no_writers_generation_count_dds : in std_logic_vector(NO_WRITERS_GENERATION_COUNT_WIDTH-1 downto 0);
si_sample_rank_dds : in std_logic_vector(SAMPLE_RANK_WIDTH-1 downto 0);
si_generation_rank_dds : in std_logic_vector(GENERATION_RANK_WIDTH-1 downto 0);
si_absolute_generation_rank_dds : in std_logic_vector(ABSOLUTE_GENERATION_COUNT_WIDTH-1 downto 0);
si_valid_data_dds : in std_logic;
si_valid_dds : in std_logic;
si_ack_dds : out std_logic;
eoc_dds : in std_logic;
-- Communication Status
status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- TO USER ENTITY
start_user : in std_logic;
opcode_user : in ROS_TOPIC_OPCODE_TYPE;
ack_user : out std_logic;
done_user : out std_logic;
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
data_available_user : out std_logic;
-- ###GENERATED START###
goal_id_addr : in std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0);
goal_id_ready : out std_logic;
goal_id_ren : in std_logic;
goal_id_valid : out std_logic;
goal_id_ack : in std_logic;
goal_id : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
seq_len : out std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0);
seq_addr : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0);
seq_ready : out std_logic;
seq_ren : in std_logic;
seq_valid : out std_logic;
seq_ack : in std_logic;
seq : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
-- ###GENERATED END###
message_info_user : out MESSAGE_INFO_TYPE;
taken_user : out std_logic
);
end entity;
architecture arch of Fibonacci_action_feedback_sub is
--*****TYPE DECLARATION*****
-- FSM states. Explained below in detail
type STAGE_TYPE is (IDLE,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,RETURN_ROS);
-- ###GENERATED START###
type DECODE_STAGE_TYPE is (GET_GOAL_ID,GET_SEQ_LENGTH,GET_SEQ,GET_OPTIONAL_HEADER);
-- ###GENERATED END###
-- *MAIN PROCESS*
signal stage, stage_next : STAGE_TYPE;
signal cnt, cnt_next : natural range 0 to 5;
signal endian_flag, endian_flag_next : std_logic;
signal last_word_in_latch, last_word_in_latch_next : std_logic;
signal decode_error_latch, decode_error_latch_next : std_logic;
signal align_offset, align_offset_next : unsigned(MAX_ALIGN_OFFSET_WIDTH-1 downto 0);
signal align_op, align_op_next : std_logic;
signal target_align, target_align_next : ALIGN_TYPE;
signal data_in_latch, data_in_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
signal optional, optional_next : std_logic;
signal abort_mem : std_logic;
signal ready_in_dds_sig : std_logic;
signal taken_sig, taken_sig_next : std_logic;
signal data_available_sig, data_available_sig_next : std_logic;
signal message_info_sig, message_info_sig_next : MESSAGE_INFO_TYPE;
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE;
signal return_stage, return_stage_next : DECODE_STAGE_TYPE;
-- ###GENERATED START###
signal goal_id_cnt, goal_id_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1;
signal seq_cnt, seq_cnt_next : natural range 0 to SEQ_MAX_DEPTH-1;
signal seq_len_latch, seq_len_latch_next : unsigned(SEQ_ADDR_WIDTH-1 downto 0);
-- seq_mem SIGNALS
signal goal_id_mem_addr : std_logic_vector(GOAL_ID_ADDR_WIDTH-1 downto 0);
signal goal_id_mem_read, goal_id_mem_ready_in, goal_id_mem_ready_out, goal_id_mem_valid_in, goal_id_mem_valid_out : std_logic;
signal goal_id_mem_data_in, goal_id_mem_data_out : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- seq_mem SIGNALS
signal seq_mem_addr : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0);
signal seq_mem_read, seq_mem_ready_in, seq_mem_ready_out, seq_mem_valid_in, seq_mem_valid_out : std_logic;
signal seq_mem_data_in, seq_mem_data_out : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
-- ###GENERATED END###
--*****ALIAS DECLARATION*****
alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_dds(WORD_WIDTH-1 downto WORD_WIDTH-PAYLOAD_REPRESENTATION_ID_WIDTH);
alias representation_options : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_dds(PAYLOAD_REPRESENTATION_OPTIONS_WIDTH-1 downto 0);
alias parameter_id : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) is data_in_latch(WORD_WIDTH-1 downto WORD_WIDTH-PARAMETER_ID_WIDTH);
alias parameter_length : std_logic_vector(PARAMETER_LENGTH_WIDTH-1 downto 0) is data_in_latch(PARAMETER_LENGTH_WIDTH-1 downto 0);
begin
-- ###GENERATED START###
goal_id_mem : entity work.mem_ctrl(arch)
generic map (
ADDR_WIDTH => GOAL_ID_ADDR_WIDTH,
DATA_WIDTH => CDR_INT8_WIDTH,
MEMORY_DEPTH => GOAL_ID_MAX_DEPTH,
MAX_BURST_LENGTH => 1
)
port map (
clk => clk,
reset => reset or abort_mem,
addr => goal_id_mem_addr,
read => goal_id_mem_read,
ready_in => goal_id_mem_ready_in,
valid_in => goal_id_mem_valid_in,
data_in => goal_id_mem_data_in,
ready_out => goal_id_mem_ready_out,
valid_out => goal_id_mem_valid_out,
data_out => goal_id_mem_data_out
);
seq_mem : entity work.mem_ctrl(arch)
generic map (
ADDR_WIDTH => SEQ_ADDR_WIDTH,
DATA_WIDTH => CDR_LONG_WIDTH,
MEMORY_DEPTH => SEQ_MAX_DEPTH,
MAX_BURST_LENGTH => 1
)
port map (
clk => clk,
reset => reset or abort_mem,
addr => seq_mem_addr,
read => seq_mem_read,
ready_in => seq_mem_ready_in,
valid_in => seq_mem_valid_in,
data_in => seq_mem_data_in,
ready_out => seq_mem_ready_out,
valid_out => seq_mem_valid_out,
data_out => seq_mem_data_out
);
-- ###GENERATED END###
-- PASSTHROUGH
taken_user <= taken_sig;
ready_in_dds <= ready_in_dds_sig;
message_info_user <= message_info_sig;
data_available_user <= data_available_sig;
instance_state_dds <= ANY_INSTANCE_STATE;
view_state_dds <= ANY_VIEW_STATE;
sample_state_dds <= ANY_SAMPLE_STATE;
instance_handle_dds <= HANDLE_NIL;
max_samples_dds <= (others => '0');
-- ###GENERATED START###
goal_id_valid <= goal_id_mem_valid_out;
goal_id <= goal_id_mem_data_out;
seq_len <= std_logic_vector(seq_len_latch);
seq_valid <= seq_mem_valid_out;
seq <= seq_mem_data_out;
-- ###GENERATED END###
main_prc : process (all)
variable tmp_length : unsigned(WORD_WIDTH-1 downto 0);
begin
-- DEFAULT
stage_next <= stage;
decode_stage_next <= decode_stage;
return_stage_next <= return_stage;
cnt_next <= cnt;
endian_flag_next <= endian_flag;
last_word_in_latch_next <= last_word_in_latch;
decode_error_latch_next <= decode_error_latch;
align_offset_next <= align_offset;
target_align_next <= target_align;
optional_next <= optional;
taken_sig_next <= taken_sig;
data_in_latch_next <= data_in_latch;
align_op_next <= align_op;
data_available_sig_next <= data_available_sig;
return_code_latch_next <= return_code_latch;
message_info_sig_next <= message_info_sig;
abort_mem <= '0';
ready_in_dds_sig <= '0';
si_ack_dds <= '0';
get_data_dds <= '0';
start_dds <= '0';
opcode_dds <= NOP;
ack_user <= '0';
done_user <= '0';
return_code_user <= ROS_RET_OK;
-- ###GENERATED START###
goal_id_cnt_next <= goal_id_cnt;
goal_id_mem_addr <= (others => '0');
goal_id_mem_read <= '0';
goal_id_mem_valid_in <= '0';
goal_id_mem_ready_out <= '0';
goal_id_mem_data_in <= (others => '0');
goal_id_ready <= '0';
seq_len_latch_next <= seq_len_latch;
seq_cnt_next <= seq_cnt;
seq_mem_addr <= (others => '0');
seq_mem_read <= '0';
seq_mem_valid_in <= '0';
seq_mem_ready_out <= '0';
seq_mem_data_in <= (others => '0');
seq_ready <= '0';
-- ###GENERATED END###
-- Last Word Latch Setter
if (last_word_in_dds = '1') then
last_word_in_latch_next <= '1';
end if;
-- Data Available Setter
if (check_mask(status_dds, DATA_AVAILABLE_STATUS)) then
data_available_sig_next <= '1';
end if;
case (stage) is
when IDLE =>
if (start_user = '1') then
ack_user <= '1';
case (opcode_user) is
when TAKE =>
stage_next <= INITIATE_READ;
when others =>
return_code_latch_next <= ROS_RET_UNSUPPORTED;
stage_next <= RETURN_ROS;
end case;
-- RESET
taken_sig_next <= '0';
abort_mem <= '1';
else
-- ###GENERATED START###
goal_id_ready <= goal_id_mem_ready_in;
goal_id_mem_addr <= goal_id_addr;
goal_id_mem_read <= goal_id_ren;
goal_id_mem_valid_in <= goal_id_ren;
goal_id_mem_ready_out <= goal_id_ack;
seq_ready <= seq_mem_ready_in;
seq_mem_addr <= seq_addr;
seq_mem_read <= seq_ren;
seq_mem_valid_in <= seq_ren;
seq_mem_ready_out <= seq_ack;
-- ###GENERATED END###
end if;
when RETURN_ROS =>
done_user <= '1';
return_code_user <= return_code_latch;
-- DONE
stage_next <= IDLE;
when INITIATE_READ =>
start_dds <= '1';
opcode_dds <= TAKE_NEXT_SAMPLE;
if (ack_dds = '1') then
stage_next <= WAIT_FOR_READER;
end if;
when WAIT_FOR_READER =>
if (done_dds = '1') then
case (return_code_dds) is
when RETCODE_OK =>
stage_next <= WAIT_FOR_DATA;
when RETCODE_NO_DATA =>
assert (taken_sig = '0') severity FAILURE;
-- Data Available Resetter
data_available_sig_next <= '0';
return_code_latch_next <= ROS_RET_OK;
stage_next <= RETURN_ROS;
when others =>
return_code_latch_next <= ROS_RET_ERROR;
stage_next <= RETURN_ROS;
end case;
end if;
when WAIT_FOR_DATA =>
if (si_valid_dds = '1') then
si_ack_dds <= '1';
-- Meta Sample
if (si_valid_data_dds = '0') then
-- Ignore and read Next Sample
stage_next <= INITIATE_READ;
else
get_data_dds <= '1';
stage_next <= GET_PAYLOAD_HEADER;
message_info_sig_next.publisher_gid <= to_gid(GUID_UNKNOWN);
message_info_sig_next.received_timestamp <= TIME_INVALID;
message_info_sig_next.source_timestamp <= si_source_timestamp_dds;
end if;
end if;
when GET_PAYLOAD_HEADER =>
-- TODO: Latch Offset from Options Field?
ready_in_dds_sig <= '1';
-- Input Guard
if (valid_in_dds = '1') then
case (representation_id) is
when CDR_BE =>
endian_flag_next <= '0';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
-- ###GENERATED START###
decode_stage_next <= GET_GOAL_ID;
goal_id_cnt_next <= 0;
-- ###GENERATED END###
-- Initial Fetch
when CDR_LE =>
endian_flag_next <= '1';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
-- ###GENERATED START###
decode_stage_next <= GET_GOAL_ID;
goal_id_cnt_next <= 0;
-- ###GENERATED END###
when others =>
-- Unknown Payload Encoding
stage_next <= SKIP_PAYLOAD;
decode_error_latch_next <= '1';
end case;
end if;
when FETCH =>
ready_in_dds_sig <= '1';
-- Input Guard
if (valid_in_dds = '1') then
data_in_latch_next <= data_in_dds;
-- Alignment Operation in progress
if (align_op = '1') then
stage_next <= ALIGN_STREAM;
-- Reset
align_op_next <= '0';
else
stage_next <= DECODE_PAYLOAD;
end if;
end if;
when ALIGN_STREAM =>
-- Target Stream Alignment reached
if (check_align(align_offset, target_align)) then
-- DONE
stage_next <= DECODE_PAYLOAD;
else
align_offset_next <= align_offset + 1;
-- Need to fetch new Input Word
if (align_offset(1 downto 0) = "11") then
align_op_next <= '1';
stage_next <= FETCH;
end if;
end if;
when DECODE_PAYLOAD =>
case (decode_stage) is
-- ###GENERATED START###
when GET_GOAL_ID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_STREAM;
else
goal_id_mem_addr <= std_logic_vector(to_unsigned(goal_id_cnt,GOAL_ID_ADDR_WIDTH));
goal_id_mem_data_in <= endian_swap(endian_flag, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE));
goal_id_mem_valid_in <= '1';
-- Memory Operation Guard
if (goal_id_mem_ready_in = '1') then
align_offset_next <= align_offset + 1;
-- Need to fetch next Word
if(align_offset(1 downto 0) = "11") then
stage_next <= FETCH;
end if;
if (goal_id_cnt = GOAL_ID_MAX_DEPTH-1) then
decode_stage_next <= GET_SEQ_LENGTH;
else
goal_id_cnt_next <= goal_id_cnt + 1;
end if;
end if;
end if;
when GET_SEQ_LENGTH =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_STREAM;
else
tmp_length := unsigned(endian_swap(endian_flag, data_in_latch));
align_offset_next <= align_offset + 4;
stage_next <= FETCH;
-- Empty Sequence
if (tmp_length = 0) then
-- DONE
stage_next <= SKIP_PAYLOAD;
else
if (tmp_length > SEQ_MAX_DEPTH) then
seq_len_latch_next <= to_unsigned(SEQ_MAX_DEPTH, seq_len_latch_next'length);
else
seq_len_latch_next <= resize(tmp_length, seq_len_latch_next'length);
end if;
seq_cnt_next <= 0;
decode_stage_next <= GET_SEQ;
end if;
end if;
when GET_SEQ =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_STREAM;
else
seq_mem_addr <= std_logic_vector(to_unsigned(seq_cnt,SEQ_ADDR_WIDTH));
seq_mem_data_in <= endian_swap(endian_flag, data_in_latch);
seq_mem_valid_in <= '1';
-- Memory Operation Guard
if (seq_mem_ready_in = '1') then
align_offset_next <= align_offset + 4;
stage_next <= FETCH;
-- All Elements processed
if (seq_cnt = seq_len_latch-1) then
-- DONE
stage_next <= SKIP_PAYLOAD;
else
seq_cnt_next <= seq_cnt + 1;
end if;
end if;
end if;
-- ###GENERATED END###
when GET_OPTIONAL_HEADER =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_STREAM;
else
case (cnt) is
-- Optional Member Header
when 0 =>
-- Extended Parameter Header
if (endian_swap(endian_flag,parameter_id) = PID_EXTENDED) then
cnt_next <= cnt + 1;
stage_next <= FETCH;
else
stage_next <= FETCH;
decode_stage_next <= return_stage;
cnt_next <= 0;
-- Alignment Reset
align_offset_next <= (others => '0');
-- Optional omitted
if(endian_swap(endian_flag,parameter_length) = (parameter_length'reverse_range => '0')) then
optional_next <= '0';
else
optional_next <= '1';
end if;
end if;
-- eMemberHeader
when 1 =>
-- Ignore Parameter ID
cnt_next <= cnt + 1;
stage_next <= FETCH;
-- Llength
when 2 =>
stage_next <= FETCH;
decode_stage_next <= return_stage;
cnt_next <= 0;
-- Alignment Reset
align_offset_next <= (others => '0');
-- Optional omitted
if(endian_swap(endian_flag, data_in_dds) = (data_in_dds'reverse_range => '0')) then
optional_next <= '0';
else
optional_next <= '1';
end if;
when others =>
null;
end case;
end if;
when others =>
null;
end case;
when SKIP_PAYLOAD =>
if (last_word_in_latch = '0' and last_word_in_dds = '0') then
-- Skip Read
ready_in_dds_sig <= '1';
else
-- Reset
last_word_in_latch_next <= '0';
-- If no Decode Error, mark output as valid
if (decode_error_latch = '0') then
taken_sig_next <= '1';
return_code_latch_next <= ROS_RET_OK;
else
taken_sig_next <= '0';
return_code_latch_next <= ROS_RET_ERROR;
end if;
stage_next <= RETURN_ROS;
end if;
end case;
-- OVERREAD GUARD
-- Attempted read on empty input
if (last_word_in_latch = '1' and last_word_in_dds = '0' and ready_in_dds_sig = '1') then
stage_next <= SKIP_PAYLOAD;
decode_error_latch_next <= '1';
end if;
end process;
sync_prc : process(clk)
begin
if rising_edge(clk) then
if (reset = '1') then
stage <= IDLE;
decode_stage <= GET_GOAL_ID;
return_stage <= GET_GOAL_ID;
target_align <= ALIGN_1;
return_code_latch <= ROS_RET_OK;
message_info_sig <= EMPTY_MESSAGE_INFO;
cnt <= 0;
endian_flag <= '0';
last_word_in_latch <= '0';
decode_error_latch <= '0';
optional <= '0';
taken_sig <= '0';
align_op <= '0';
data_available_sig <= '0';
align_offset <= (others => '0');
data_in_latch <= (others => '0');
-- ###GENERATED START###
goal_id_cnt <= 0;
seq_cnt <= 0;
seq_len_latch <= (others => '0');
-- ###GENERATED END###
else
stage <= stage_next;
decode_stage <= decode_stage_next;
return_stage <= return_stage_next;
target_align <= target_align_next;
return_code_latch <= return_code_latch_next;
message_info_sig <= message_info_sig_next;
cnt <= cnt_next;
endian_flag <= endian_flag_next;
last_word_in_latch <= last_word_in_latch_next;
decode_error_latch <= decode_error_latch_next;
optional <= optional_next;
taken_sig <= taken_sig_next;
align_op <= align_op_next;
data_available_sig <= data_available_sig_next;
align_offset <= align_offset_next;
data_in_latch <= data_in_latch_next;
-- ###GENERATED START###
goal_id_cnt <= goal_id_cnt_next;
seq_cnt <= seq_cnt_next;
seq_len_latch <= seq_len_latch_next;
-- ###GENERATED END###
end if;
end if;
end process;
end architecture;

19
src/ros2/example_interfaces/Fibonacci_package.vhd Normal file
View File

@ -0,0 +1,19 @@
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.math_pkg.all;
use work.ros_package.all;
package Fibonacci_package is
constant GOAL_ID_MAX_DEPTH : natural := 16;
constant GOAL_ID_ADDR_WIDTH : natural := log2c(GOAL_ID_MAX_DEPTH);
constant SEQ_MAX_DEPTH : natural := 100;
constant SEQ_ADDR_WIDTH : natural := log2c(SEQ_MAX_DEPTH);
constant MAX_GOAL_ID_SIZE : natural := GOAL_ID_MAX_DEPTH;
constant MAX_SEQ_SIZE : natural := SEQ_MAX_DEPTH * 4;
constant MAX_FIBONACCI_SIZE : natural := MAX_GOAL_ID_SIZE + MAX_SEQ_SIZE;
end package;

88
src/ros2/ros_package.vhd
View File

@ -157,6 +157,20 @@ package ros_package is
constant EMPTY_SERVICE_INFO : SERVICE_INFO_TYPE := (source_timestamp => TIME_INVALID, received_timestamp => TIME_INVALID, request_id => EMPTY_REQUEST_ID); constant EMPTY_SERVICE_INFO : SERVICE_INFO_TYPE := (source_timestamp => TIME_INVALID, received_timestamp => TIME_INVALID, request_id => EMPTY_REQUEST_ID);
constant GID_WIDTH : natural := 192;
type GID_TYPE is array (0 to (GID_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0);
function to_gid(guid : GUID_TYPE) return GID_TYPE;
type MESSAGE_INFO_TYPE is record
source_timestamp : TIME_TYPE;
received_timestamp : TIME_TYPE;
publisher_gid : GID_TYPE;
from_intra_process : boolean;
end record;
constant EMPTY_MESSAGE_INFO : MESSAGE_INFO_TYPE;-- Deferred to Package Body
type ROS_NODE_ARRAY_TYPE is array (natural range <>) of ROS_NODE_TYPE; type ROS_NODE_ARRAY_TYPE is array (natural range <>) of ROS_NODE_TYPE;
type ROS_TOPIC_ARRAY_TYPE is array (natural range <>) of ROS_TOPIC_TYPE; type ROS_TOPIC_ARRAY_TYPE is array (natural range <>) of ROS_TOPIC_TYPE;
type ROS_SERVICE_ARRAY_TYPE is array (natural range <>) of ROS_SERVICE_TYPE; type ROS_SERVICE_ARRAY_TYPE is array (natural range <>) of ROS_SERVICE_TYPE;
@ -164,22 +178,76 @@ package ros_package is
-- *ROS RETURN CODES* -- *ROS RETURN CODES*
constant ROS_RETCODE_WIDTH : natural := 32; constant ROS_RETCODE_WIDTH : natural := 32;
constant ROS_RET_OK : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(0,ROS_RETCODE_WIDTH)); constant ROS_RET_OK : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(0,ROS_RETCODE_WIDTH));
constant ROS_RET_ERROR : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1,ROS_RETCODE_WIDTH)); constant ROS_RET_ERROR : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1,ROS_RETCODE_WIDTH));
constant ROS_RET_TIMEOUT : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2,ROS_RETCODE_WIDTH)); constant ROS_RET_TIMEOUT : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2,ROS_RETCODE_WIDTH));
constant ROS_RET_UNSUPPORTED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3,ROS_RETCODE_WIDTH)); constant ROS_RET_UNSUPPORTED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3,ROS_RETCODE_WIDTH));
constant ROS_RET_BAD_ALLOC : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(10,ROS_RETCODE_WIDTH));
constant ROS_RET_INVALID_ARGUMENT : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(11,ROS_RETCODE_WIDTH));
constant ROS_RET_INCORRECT_RMW_IMPLEMENTATION : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(12,ROS_RETCODE_WIDTH));
-- RCL 1xx
constant ROS_RET_ALREADY_INIT : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(100,ROS_RETCODE_WIDTH));
constant ROS_RET_NOT_INIT : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(101,ROS_RETCODE_WIDTH));
constant ROS_RET_MISMATCHED_RMW : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(102,ROS_RETCODE_WIDTH));
constant ROS_RET_TOPIC_NAME_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(103,ROS_RETCODE_WIDTH));
constant ROS_RET_SERVICE_NAME_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(104,ROS_RETCODE_WIDTH));
constant ROS_RET_UNKNOWN_SUBSTITUTION : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(105,ROS_RETCODE_WIDTH));
constant ROS_RET_ALREADY_SHUTDOWN : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(106,ROS_RETCODE_WIDTH));
-- RCL NODE 2xx
constant ROS_RET_NODE_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(200,ROS_RETCODE_WIDTH));
constant ROS_RET_NODE_INVALID_NAME : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(201,ROS_RETCODE_WIDTH));
constant ROS_RET_NODE_INVALID_NAMESPACE : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(202,ROS_RETCODE_WIDTH));
constant ROS_RET_NODE_NAME_NON_EXISTENT : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(203,ROS_RETCODE_WIDTH));
-- RCL PUBLISHER 3xx
constant ROS_RET_PUBLISHER_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(300,ROS_RETCODE_WIDTH));
-- RCL SUBSCRIPTION 4xx
constant ROS_RET_SUBSCRIPTION_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(400,ROS_RETCODE_WIDTH));
constant ROS_RET_SUBSCRIPTION_TAKE_FAILED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(401,ROS_RETCODE_WIDTH));
-- RCL SERVICE CLIENT 5xx
constant ROS_RET_CLIENT_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(500,ROS_RETCODE_WIDTH));
constant ROS_RET_CLIENT_TAKE_FAILED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(501,ROS_RETCODE_WIDTH));
-- RCL SERVICE SERVER 6xx
constant ROS_RET_SERVICE_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(600,ROS_RETCODE_WIDTH));
constant ROS_RET_SERVICE_TAKE_FAILED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(601,ROS_RETCODE_WIDTH));
-- RCL GUARD 7xx
-- RCL TIMER 8xx
constant ROS_RET_TIMER_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(800,ROS_RETCODE_WIDTH));
constant ROS_RET_TIMER_CANCELED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(801,ROS_RETCODE_WIDTH));
-- RCL WAIT 9xx
constant ROS_RET_WAIT_SET_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(900,ROS_RETCODE_WIDTH));
constant ROS_RET_WAIT_SET_EMPTY : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(901,ROS_RETCODE_WIDTH));
constant ROS_RET_WAIT_SET_FULL : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(902,ROS_RETCODE_WIDTH));
-- RCL ARGUMENT PARSING 1xxx
constant ROS_RET_INVALID_REMAP_RULE : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1001,ROS_RETCODE_WIDTH));
constant ROS_RET_WRONG_LEXEME : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1002,ROS_RETCODE_WIDTH));
constant ROS_RET_INVALID_ROS_ARGS : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1003,ROS_RETCODE_WIDTH));
constant ROS_RET_INVALID_PARAM_RULE : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1010,ROS_RETCODE_WIDTH));
constant ROS_RET_INVALID_LOG_LEVEL_RULE : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1020,ROS_RETCODE_WIDTH));
-- RCL EVENT 20xx
constant ROS_RET_EVENT_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2000,ROS_RETCODE_WIDTH));
constant ROS_RET_EVENT_TAKE_FAILED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2001,ROS_RETCODE_WIDTH));
-- RCL ACTION 2xxx
constant ROS_RET_ACTION_NAME_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2000,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_GOAL_ACCEPTED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2100,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_GOAL_REJECTED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2101,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_CLIENT_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2102,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_CLIENT_TAKE_FAILED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2103,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_SERVER_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2200,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_SERVER_TAKE_FAILED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2201,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_GOAL_HANDLE_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2300,ROS_RETCODE_WIDTH));
constant ROS_RET_ACTION_GOAL_EVENT_INVALID : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2301,ROS_RETCODE_WIDTH));
-- RCL LIFECYCLE STATE REGISTER 30xx
constant ROS_RET_LIFECYCLE_STATE_REGISTERED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3000,ROS_RETCODE_WIDTH));
constant ROS_RET_LIFECYCLE_STATE_NOT_REGISTERED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3001,ROS_RETCODE_WIDTH));
type ROS_TOPIC_OPCODE_TYPE is (NOP, PUBLISH, TAKE);
type ROS_SERVICE_OPCODE_TYPE is (NOP, SEND_REQUEST, TAKE_REQUEST, SEND_RESPONSE, TAKE_RESPONSE); type ROS_SERVICE_OPCODE_TYPE is (NOP, SEND_REQUEST, TAKE_REQUEST, SEND_RESPONSE, TAKE_RESPONSE);
type ROS_ACTION_OPCODE_TYPE is (NOP, SEND_GOAL_REQUEST, TAKE_GOAL_REQUEST, SEND_GOAL_RESPONSE, TAKE_GOAL_RESPONSE, SEND_RESULT_REQUEST, TAKE_RESULT_REQUEST, SEND_RESULT_RESPONSE, TAKE_RESULT_RESPONSE, SEND_CANCEL_REQUEST, TAKE_CANCEL_REQUEST, SEND_CANCEL_RESPONSE, TAKE_CANCEL_RESPONSE, PUBLISH_FEEDBACK, ACCEPT_GOAL, UPDATE_GOAL, EXPIRE_GOAL);
constant ROS_SEQUENCE_ID_WIDTH : natural := 64; constant ROS_SEQUENCE_ID_WIDTH : natural := 64;
type ENDPOINT_ROS_NODE_MAPPING_ARRAY_TYPE is array (natural range <>) of natural; type ENDPOINT_ROS_NODE_MAPPING_ARRAY_TYPE is array (natural range <>) of natural;
constant GID_WIDTH : natural := 192;
type GID_TYPE is array (0 to (GID_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0);
function to_gid(guid : GUID_TYPE) return GID_TYPE;
constant SERVICE_OVERHEAD_BYTES : natural := 16; constant SERVICE_OVERHEAD_BYTES : natural := 16;
function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural; function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural;
@ -260,6 +328,8 @@ package body ros_package is
return ret; return ret;
end function; end function;
constant EMPTY_MESSAGE_INFO : MESSAGE_INFO_TYPE := (source_timestamp => TIME_INVALID, received_timestamp => TIME_INVALID, publisher_gid => to_gid(GUID_UNKNOWN), from_intra_process => FALSE);
function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural is function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural is
variable ret : natural; variable ret : natural;
begin begin