diff --git a/sim/L1_Fibonacci_action_feedback_test1.do b/sim/L1_Fibonacci_action_feedback_test1.do new file mode 100644 index 0000000..5eab71e --- /dev/null +++ b/sim/L1_Fibonacci_action_feedback_test1.do @@ -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} diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd new file mode 100644 index 0000000..20cd112 --- /dev/null +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_action_feedback_test1.vhd @@ -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; \ No newline at end of file diff --git a/src/ros2/Tests/ros_testbench.pro b/src/ros2/Tests/ros_testbench.pro index 18cfa66..473b6a9 100644 --- a/src/ros2/Tests/ros_testbench.pro +++ b/src/ros2/Tests/ros_testbench.pro @@ -83,13 +83,23 @@ analyze ../../TEMPLATE_user_config.vhd analyze ../../rtps_config_package.vhd analyze ../../rtps_test_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_server.vhd analyze Level_1/L1_AddTwoInts_srv_test1.vhd analyze Level_1/L1_AddTwoInts_srv_test2.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_test2 simulate L2_AddTwoInts_test1 +simulate L1_Fibonacci_action_feedback_test1 \ No newline at end of file diff --git a/src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd b/src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd new file mode 100644 index 0000000..52d27dd --- /dev/null +++ b/src/ros2/example_interfaces/Fibonacci_action_feedback_pub.vhd @@ -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; \ No newline at end of file diff --git a/src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd b/src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd new file mode 100644 index 0000000..bf91244 --- /dev/null +++ b/src/ros2/example_interfaces/Fibonacci_action_feedback_sub.vhd @@ -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; \ No newline at end of file diff --git a/src/ros2/example_interfaces/Fibonacci_package.vhd b/src/ros2/example_interfaces/Fibonacci_package.vhd new file mode 100644 index 0000000..fa7bbf5 --- /dev/null +++ b/src/ros2/example_interfaces/Fibonacci_package.vhd @@ -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; \ No newline at end of file diff --git a/src/ros2/ros_package.vhd b/src/ros2/ros_package.vhd index e27ee9b..1871af7 100644 --- a/src/ros2/ros_package.vhd +++ b/src/ros2/ros_package.vhd @@ -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 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_TOPIC_ARRAY_TYPE is array (natural range <>) of ROS_TOPIC_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* 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_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_UNSUPPORTED : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3,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_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_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_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; 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; function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural; @@ -260,6 +328,8 @@ package body ros_package is return ret; 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 variable ret : natural; begin