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

Add ROS Action TEMPLATES and generation recipes

Browse Source 
The existing Fibonacci implementation was slightly modified to adhere to
the recipes and be more in line with existing code generation.
This commit is contained in:
John Ring 2022-04-27 12:37:02 +02:00
parent e7ca8c581e
commit 6fe7e426ca
17 changed files with 4677 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
sim/L2_Fibonacci_test1.do
View File

@ -83,31 +83,31 @@ add wave -noupdate -group CLIENT -radix unsigned /l2_fibonacci_ros_action_test1/
add wave -noupdate -group CLIENT -radix hexadecimal /l2_fibonacci_ros_action_test1/client_inst/Fibonacci_ros_action_client_inst/result_goal_id add wave -noupdate -group CLIENT -radix hexadecimal /l2_fibonacci_ros_action_test1/client_inst/Fibonacci_ros_action_client_inst/result_goal_id
add wave -noupdate -group CLIENT -radix hexadecimal /l2_fibonacci_ros_action_test1/client_inst/Fibonacci_ros_action_client_inst/cancel_goal_id add wave -noupdate -group CLIENT -radix hexadecimal /l2_fibonacci_ros_action_test1/client_inst/Fibonacci_ros_action_client_inst/cancel_goal_id
add wave -noupdate -divider SERVER add wave -noupdate -divider SERVER
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/start add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/start
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/opcode add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/opcode
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/ack add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/ack
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/done add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/done
add wave -noupdate -group SERVER -radix ROS_RETCODE /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/return_code add wave -noupdate -expand -group SERVER -radix ROS_RETCODE /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/return_code
add wave -noupdate -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_handle_in add wave -noupdate -expand -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_handle_in
add wave -noupdate -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_handle_out add wave -noupdate -expand -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_handle_out
add wave -noupdate -group SERVER -radix GOAL_STATUS /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_state_in add wave -noupdate -expand -group SERVER -radix GOAL_STATUS /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_state_in
add wave -noupdate -group SERVER -radix GOAL_STATUS /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_state_out add wave -noupdate -expand -group SERVER -radix GOAL_STATUS /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_state_out
add wave -noupdate -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_id add wave -noupdate -expand -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_id
add wave -noupdate -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_result_index add wave -noupdate -expand -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_result_index
add wave -noupdate -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_stamp add wave -noupdate -expand -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/goal_stamp
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_request add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_request
add wave -noupdate -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_handle add wave -noupdate -expand -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_handle
add wave -noupdate -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_result_index add wave -noupdate -expand -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_result_index
add wave -noupdate -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_order add wave -noupdate -expand -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_order
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_accepted add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_accepted
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_response add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/new_goal_response
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_request add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_request
add wave -noupdate -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_request_handle add wave -noupdate -expand -group SERVER -radix hexadecimal /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_request_handle
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_accepted add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_accepted
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_response add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/cancel_response
add wave -noupdate -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/index_result add wave -noupdate -expand -group SERVER -radix unsigned /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/result_addr
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/index_result_wen add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/result_ready
add wave -noupdate -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/index_result_ready add wave -noupdate -expand -group SERVER /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/result_wen
add wave -noupdate -divider MISC add wave -noupdate -divider MISC
add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_inst/stage add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_inst/stage
add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_inst/cnt add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_inst/cnt
@ -136,7 +136,7 @@ add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_acti
add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_opcode add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_opcode
add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_stage add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_stage
add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_cnt add wave -noupdate /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_cnt
add wave -noupdate -radix hexadecimal -childformat {{/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.addr -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.goal_id -radix hexadecimal} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.stamp -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.deadline -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.res_ind -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.state -radix GOAL_STATUS}} -subitemconfig {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.addr {-radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.goal_id {-radix hexadecimal} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.stamp {-radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.deadline {-radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.res_ind {-radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.state {-radix GOAL_STATUS}} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data add wave -noupdate -radix hexadecimal -childformat {{/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.addr -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.goal_id -radix hexadecimal} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.stamp -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.deadline -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.res_ind -radix unsigned} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.state -radix GOAL_STATUS} {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.field_flags -radix hexadecimal}} -subitemconfig {/l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.addr {-height 15 -radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.goal_id {-height 15 -radix hexadecimal} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.stamp {-height 15 -radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.deadline {-height 15 -radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.res_ind {-height 15 -radix unsigned} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.state {-height 15 -radix GOAL_STATUS} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data.field_flags {-radix hexadecimal}} /l2_fibonacci_ros_action_test1/server_inst/Fibonacci_ros_action_server_inst/action_server_inst/mem_data
TreeUpdate [SetDefaultTree] TreeUpdate [SetDefaultTree]
WaveRestoreCursors {{Cursor 1} {455175000 ps} 0} WaveRestoreCursors {{Cursor 1} {455175000 ps} 0}
quietly wave cursor active 1 quietly wave cursor active 1

41
src/IDL-VHDL_Ref.txt
View File

@ -96,13 +96,14 @@ SEQUENCE
-------- --------
The name of the generated memory is <NAME>_mem, and the memory signals are connected via signals of The name of the generated memory is <NAME>_mem, and the memory signals are connected via signals of
name <NAME>_mem_<SIGNALNAME>. The generated memory uses a MAX_BURST_LENGTH of 1. name <NAME>_mem_<SIGNALNAME>. The generated memory uses a MAX_BURST_LENGTH of 1.
A latch called <NAME>_len_latch is generated to hold the sequence length.
Following Port signals are defined: Following Port signals are defined:
NAME DIRECTION CONNECTED NAME DIRECTION CONNECTED
<NAME>_len out <NAME>_len_latch <NAME>_len out <NAME>_len_latch
<NAME>_addr in <NAME>_mem_addr in 'IDLE' stage <NAME>_addr in <NAME>_mem_addr in 'IDLE' stage
<NAME>_ready out <NAME>_mem_ready_in in 'IDLE' stage, else '0' [NOTE: ANDing for aggregated elment types (see Structures below)] <NAME>_ready out <NAME>_mem_ready_in in 'IDLE' stage, else '0' [NOTE: ANDing for aggregated elment types (see Structures below)]
<NAME>_ren in <NAME>_mem_read and <NAME>_mem_valid_in in 'IDLE' stage <NAME>_ren in <NAME>_mem_read and <NAME>_mem_valid_in in 'IDLE' stage
<NAME>_valid out <NAME>_mem_valid_out [NOTE: ANDing for aggregated elment types (see Structures below)] <NAME>_valid out <NAME>_mem_valid_out [NOTE: ANDing for aggregated element types (see Structures below)]
<NAME>_ack in <NAME>_mem_ready_out in 'IDLE' stage <NAME>_ack in <NAME>_mem_ready_out in 'IDLE' stage
<NAME> out <NAME>_mem_data_out <NAME> out <NAME>_mem_data_out
@ -130,7 +131,7 @@ Array is similar to the sequence, but has no length encoding (since it always ha
That means that there is no <NAME>_len port signal, <NAME>_len_latch latch, and also no That means that there is no <NAME>_len port signal, <NAME>_len_latch latch, and also no
GET_<NAME>_LENGTH stage. GET_<NAME>_LENGTH stage.
The initialization of the <NAME>_cnt has to be done in the previous decode_stage. The initialization of the <NAME>_cnt has to be done in the previous decode_stage.
The <NAME>_cnt is compared against the fixed array length constant form the type package. The <NAME>_cnt is compared against the fixed array length constant from the type package.
MAP MAP
--- ---
@ -154,7 +155,7 @@ STRUCTURE
If a type member is in itself another structure, the generated <NAME> ports,signals,memories,stages,etc If a type member is in itself another structure, the generated <NAME> ports,signals,memories,stages,etc
are split into <NAME>_<SUB-NAME> signals,ports,memories,stages,etc. are split into <NAME>_<SUB-NAME> signals,ports,memories,stages,etc.
In case the element type of a collection type is an aggregated type, the stage of the last memebr is In case the element type of a collection type is an aggregated type, the stage of the last member is
responsible for handling the <NAME>_cnt increment and check which normally be handled in the GET_<NAME> responsible for handling the <NAME>_cnt increment and check which normally be handled in the GET_<NAME>
decode_stage. The <NAME>_ready and <NAME>_valid decode_stage. The <NAME>_ready and <NAME>_valid
port signals are generated by ANDing the respective memory signals of all sub-elements. port signals are generated by ANDing the respective memory signals of all sub-elements.
@ -173,20 +174,33 @@ decode_stage of the next declared member.
NESTED COLLECTIONS NESTED COLLECTIONS
================== ==================
ARRAY
-----
If a collection type has in itself another collection type, an array of memories is generated, and the If a collection type has in itself another collection type, an array of memories is generated, and the
respective memory connection signals are also multidimensional (Meaning that custom types have to be respective memory connection signals are also multidimensional (Meaning that custom types have to be
created). created).
The <OUTER_COLLECTION_NAME>_cnt of the outer collection is used to index the multidimensional signals The <OUTER_COLLECTION_NAME>_cnt of the outer collection is used to index the multidimensional signals
of the inner collection. of the inner collection, except in the IDLE state, where the <OUTER_COLLECTION_NAME>_addr port is used
for indexing.
The <OUTER_COLLECTION_NAME>_ready and <OUTER_COLLECTION_NAME>_valid ports of the outer collection The <OUTER_COLLECTION_NAME>_ready and <OUTER_COLLECTION_NAME>_valid ports of the outer collection
are no longer dependant on the inner collection (If the outer array has no other elements type except the are no longer dependant on the inner collection (If the outer array has no other elements type except the
inner collection the ports have to be hardwired). inner collection the ports have to be hardwired).
A new <OUTER_COLLECTION_NAME>_addr_latch helper latch is defined, that latches the The <OUTER_COLLECTION_NAME>_addr port is used to index the memory to connect to the
<OUTER_COLLECTION_NAME>_addr port when the <OUTER_COLLECTION_NAME>_<INNER_COLLECTION_NAME>_ren
port signal is '1'. This address latch is used to connect the correct memory to the
<OUTER_COLLECTION_NAME>_<INNER_COLLECTION_NAME> and <OUTER_COLLECTION_NAME>_<INNER_COLLECTION_NAME> and
<OUTER_COLLECTION_NAME>_<INNER_COLLECTION_NAME>_valid port signals. <OUTER_COLLECTION_NAME>_<INNER_COLLECTION_NAME>_valid port signals.
SEQUENCE/MAP
------------
In addition to the above changes, the <INNER_COLLECTION_NAME>_len_latch is converted to a memory (since
we need to keep the lengths of all inner sequences) named <INNER_COLLECTION_NAME>_len_mem.
The GET_<INNER_COLLECTION_NAME>_LENGTH stage is responsible for setting the <INNER_COLLECTION_NAME>_len_mem.
Similar to the normal collection case, in the IDLE stage the <INNER_COLLECTION_NAME>_len_mem_read and
<INNER_COLLECTION_NAME>_len_mem_valid_in memory signals are connected to the <OUTER_COLLECTION_NAME>_ren port,
and the <INNER_COLLECTION_NAME>_len_mem_ready_out is connected to the <OUTER_COLLECTION_NAME>_ack port.
In contrast to the previous case of an array, the <OUTER_COLLECTION_NAME>_ready and
<OUTER_COLLECTION_NAME>_valid ports are now also dependant on the
<INNER_COLLECTION_NAME>_len_mem_ready_in and <INNER_COLLECTION_NAME>_len_mem_valid_out signals, respectively.
OPTIONALS OPTIONALS
========= =========
@ -240,6 +254,19 @@ NOTE: If the encode_stage already is divided into sub-stages (due to the primiti
sub-stage is added that does the Memory fetch operation, and the <NAME>_mem_ready_out is pulled high sub-stage is added that does the Memory fetch operation, and the <NAME>_mem_ready_out is pulled high
in the last sub-stage. in the last sub-stage.
NESTED COLLECTIONS
==================
SEQUENCE/MAP
------------
In case the nested collection is a sequence/map, a <INNER_COLLECTION_NAME>_len_mem memory is again needed
for storing the individual sequence lengths, similar to the reader_interface.
The memory signal connections are similar to the normal write_interface collection case, allowing the
length to be stored when a write to the outer collection happens.
The WRITE_<INNER_COLLECTION_NAME>_LENGTH stage has to be implemented, similar to the normal collection
case, with a extra sub-stage for the memory fetch operation.
OPTIONALS OPTIONALS
========= =========
The direction of the <NAME>_opt port signal is inverted (in) and the <NAME>_opt_latch is removed. The direction of the <NAME>_opt port signal is inverted (in) and the <NAME>_opt_latch is removed.

2
src/Tests/Type2_key_holder.vhd
View File

@ -69,7 +69,6 @@ architecture TYPE2 of key_holder is
signal id_latch, id_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); signal id_latch, id_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
signal TestSequence_cnt, TestSequence_cnt_next : natural range 0 to TESTSEQUENCE_MAX_DEPTH-1; signal TestSequence_cnt, TestSequence_cnt_next : natural range 0 to TESTSEQUENCE_MAX_DEPTH-1;
signal TestSequence_len_latch, TestSequence_len_latch_next : unsigned(TESTSEQUENCE_ADDR_WIDTH-1 downto 0); signal TestSequence_len_latch, TestSequence_len_latch_next : unsigned(TESTSEQUENCE_ADDR_WIDTH-1 downto 0);
signal TestSequence_addr_latch, TestSequence_addr_latch_next : std_logic_vector(TESTSEQUENCE_ADDR_WIDTH-1 downto 0);
signal TestSequence_TestArray_cnt, TestSequence_TestArray_cnt_next : natural range 0 to TESTSEQUENCE_TESTARRAY_MAX_DEPTH-1; signal TestSequence_TestArray_cnt, TestSequence_TestArray_cnt_next : natural range 0 to TESTSEQUENCE_TESTARRAY_MAX_DEPTH-1;
-- TestSequence_TestArray_mem SIGNALS -- TestSequence_TestArray_mem SIGNALS
signal TestSequence_TestArray_mem_addr : TESTSEQUENCE_TESTARRAY_ADDR_TYPE; signal TestSequence_TestArray_mem_addr : TESTSEQUENCE_TESTARRAY_ADDR_TYPE;
@ -166,7 +165,6 @@ begin
TestSequence_len_latch_next <= TestSequence_len_latch; TestSequence_len_latch_next <= TestSequence_len_latch;
TestSequence_cnt_next <= TestSequence_cnt; TestSequence_cnt_next <= TestSequence_cnt;
TestSequence_TestArray_cnt_next <= TestSequence_TestArray_cnt; TestSequence_TestArray_cnt_next <= TestSequence_TestArray_cnt;
TestSequence_addr_latch_next <= TestSequence_addr_latch;
TestSequence_TestArray_mem_addr <= (others => (others => '0')); TestSequence_TestArray_mem_addr <= (others => (others => '0'));
TestSequence_TestArray_mem_read <= (others => '0'); TestSequence_TestArray_mem_read <= (others => '0');
TestSequence_TestArray_mem_valid_in <= (others => '0'); TestSequence_TestArray_mem_valid_in <= (others => '0');

14
src/Tests/Type2_reader_interface.vhd
View File

@ -137,7 +137,6 @@ architecture arch of Type2_reader_interface is
signal id_latch, id_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); signal id_latch, id_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
signal TestSequence_cnt, TestSequence_cnt_next : natural range 0 to TESTSEQUENCE_MAX_DEPTH-1; signal TestSequence_cnt, TestSequence_cnt_next : natural range 0 to TESTSEQUENCE_MAX_DEPTH-1;
signal TestSequence_len_latch, TestSequence_len_latch_next : unsigned(TESTSEQUENCE_ADDR_WIDTH-1 downto 0); signal TestSequence_len_latch, TestSequence_len_latch_next : unsigned(TESTSEQUENCE_ADDR_WIDTH-1 downto 0);
signal TestSequence_addr_latch, TestSequence_addr_latch_next : std_logic_vector(TESTSEQUENCE_ADDR_WIDTH-1 downto 0);
signal TestSequence_TestArray_cnt, TestSequence_TestArray_cnt_next : natural range 0 to TESTSEQUENCE_TESTARRAY_MAX_DEPTH-1; signal TestSequence_TestArray_cnt, TestSequence_TestArray_cnt_next : natural range 0 to TESTSEQUENCE_TESTARRAY_MAX_DEPTH-1;
signal TestMap_cnt, TestMap_cnt_next : natural range 0 to TESTMAP_MAX_DEPTH-1; signal TestMap_cnt, TestMap_cnt_next : natural range 0 to TESTMAP_MAX_DEPTH-1;
signal TestMap_len_latch, TestMap_len_latch_next : unsigned(TESTMAP_ADDR_WIDTH-1 downto 0); signal TestMap_len_latch, TestMap_len_latch_next : unsigned(TESTMAP_ADDR_WIDTH-1 downto 0);
@ -383,10 +382,8 @@ begin
id <= id_latch; id <= id_latch;
TestSequence_len <= std_logic_vector(TestSequence_len_latch); TestSequence_len <= std_logic_vector(TestSequence_len_latch);
TestSequence_valid <= TestSequence_TestChar_mem_valid_out and TestSequence_TestWChar_mem_valid_out and TestSequence_TestLongLong_mem_valid_out and TestSequence_TestLongDouble_mem_valid_out; TestSequence_valid <= TestSequence_TestChar_mem_valid_out and TestSequence_TestWChar_mem_valid_out and TestSequence_TestLongLong_mem_valid_out and TestSequence_TestLongDouble_mem_valid_out;
-- NOTE: We could also solve this with a one-hot mux, but syntesis of a one-hot mux is more complex than a simple index mux. TestSequence_TestArray <= TestSequence_TestArray_mem_data_out(to_integer(unsigned(TestSequence_addr)));
-- Alternatively you could just OR all outputs, but that places the restriction that the data_out signal has to be (others => '0') when valid = '0'. TestSequence_TestArray_valid <= TestSequence_TestArray_mem_valid_out(to_integer(unsigned(TestSequence_addr)));
TestSequence_TestArray <= TestSequence_TestArray_mem_data_out(to_integer(unsigned(TestSequence_addr_latch)));
TestSequence_TestArray_valid <= TestSequence_TestArray_mem_valid_out(to_integer(unsigned(TestSequence_addr_latch)));
TestSequence_TestChar <= TestSequence_TestChar_mem_data_out; TestSequence_TestChar <= TestSequence_TestChar_mem_data_out;
TestSequence_TestWChar <= TestSequence_TestWChar_mem_data_out; TestSequence_TestWChar <= TestSequence_TestWChar_mem_data_out;
TestSequence_TestLongLong <= TestSequence_TestLongLong_mem_data_out; TestSequence_TestLongLong <= TestSequence_TestLongLong_mem_data_out;
@ -433,7 +430,6 @@ begin
TestSequence_len_latch_next <= TestSequence_len_latch; TestSequence_len_latch_next <= TestSequence_len_latch;
TestSequence_cnt_next <= TestSequence_cnt; TestSequence_cnt_next <= TestSequence_cnt;
TestSequence_TestArray_cnt_next <= TestSequence_TestArray_cnt; TestSequence_TestArray_cnt_next <= TestSequence_TestArray_cnt;
TestSequence_addr_latch_next <= TestSequence_addr_latch;
TestMap_cnt_next <= TestMap_cnt; TestMap_cnt_next <= TestMap_cnt;
TestMap_len_latch_next <= TestMap_len_latch; TestMap_len_latch_next <= TestMap_len_latch;
TestEnum_latch_next <= TestEnum_latch; TestEnum_latch_next <= TestEnum_latch;
@ -528,10 +524,6 @@ begin
TestSequence_TestArray_mem_read(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren; TestSequence_TestArray_mem_read(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren;
TestSequence_TestArray_mem_valid_in(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren; TestSequence_TestArray_mem_valid_in(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren;
TestSequence_TestArray_mem_ready_out <= (others => TestSequence_TestArray_ack); TestSequence_TestArray_mem_ready_out <= (others => TestSequence_TestArray_ack);
-- Latch Address for output
if (TestSequence_TestArray_ren = '1') then
TestSequence_addr_latch_next <= TestSequence_addr;
end if;
TestMap_ready <= TestMap_key_mem_ready_in and TestMap_value_mem_ready_in; TestMap_ready <= TestMap_key_mem_ready_in and TestMap_value_mem_ready_in;
TestMap_key_mem_addr <= TestMap_addr; TestMap_key_mem_addr <= TestMap_addr;
TestMap_key_mem_read <= TestMap_ren; TestMap_key_mem_read <= TestMap_ren;
@ -1140,7 +1132,6 @@ begin
TestSequence_len_latch <= (others => '0'); TestSequence_len_latch <= (others => '0');
TestSequence_cnt <= 0; TestSequence_cnt <= 0;
TestSequence_TestArray_cnt <= 0; TestSequence_TestArray_cnt <= 0;
TestSequence_addr_latch <= (others => '0');
TestMap_cnt <= 0; TestMap_cnt <= 0;
TestMap_len_latch <= (others => '0'); TestMap_len_latch <= (others => '0');
TestEnum_latch <= (others => '0'); TestEnum_latch <= (others => '0');
@ -1173,7 +1164,6 @@ begin
TestSequence_len_latch <= TestSequence_len_latch_next; TestSequence_len_latch <= TestSequence_len_latch_next;
TestSequence_cnt <= TestSequence_cnt_next; TestSequence_cnt <= TestSequence_cnt_next;
TestSequence_TestArray_cnt <= TestSequence_TestArray_cnt_next; TestSequence_TestArray_cnt <= TestSequence_TestArray_cnt_next;
TestSequence_addr_latch <= TestSequence_addr_latch_next;
TestMap_cnt <= TestMap_cnt_next; TestMap_cnt <= TestMap_cnt_next;
TestMap_len_latch <= TestMap_len_latch_next; TestMap_len_latch <= TestMap_len_latch_next;
TestEnum_latch <= TestEnum_latch_next; TestEnum_latch <= TestEnum_latch_next;

28
src/Tests/Type2_writer_interface.vhd
View File

@ -134,7 +134,6 @@ architecture arch of Type2_writer_interface is
-- ###GENERATED START### -- ###GENERATED START###
signal TestSequence_cnt, TestSequence_cnt_next : natural range 0 to TESTSEQUENCE_MAX_DEPTH-1; signal TestSequence_cnt, TestSequence_cnt_next : natural range 0 to TESTSEQUENCE_MAX_DEPTH-1;
signal TestSequence_addr_latch, TestSequence_addr_latch_next : std_logic_vector(TESTSEQUENCE_ADDR_WIDTH-1 downto 0);
signal TestSequence_TestArray_cnt, TestSequence_TestArray_cnt_next : natural range 0 to TESTSEQUENCE_TESTARRAY_MAX_DEPTH-1; signal TestSequence_TestArray_cnt, TestSequence_TestArray_cnt_next : natural range 0 to TESTSEQUENCE_TESTARRAY_MAX_DEPTH-1;
signal TestMap_cnt, TestMap_cnt_next : natural range 0 to TESTMAP_MAX_DEPTH-1; signal TestMap_cnt, TestMap_cnt_next : natural range 0 to TESTMAP_MAX_DEPTH-1;
signal TestString_cnt, TestString_cnt_next : natural range 0 to TESTSTRING_MAX_DEPTH-1; signal TestString_cnt, TestString_cnt_next : natural range 0 to TESTSTRING_MAX_DEPTH-1;
@ -354,27 +353,25 @@ begin
-- ###GENERATED START### -- ###GENERATED START###
TestSequence_valid <= TestSequence_TestChar_mem_valid_out and TestSequence_TestWChar_mem_valid_out and TestSequence_TestLongLong_mem_valid_out and TestSequence_TestLongDouble_mem_valid_out; TestSequence_valid <= TestSequence_TestChar_mem_valid_out and TestSequence_TestWChar_mem_valid_out and TestSequence_TestLongLong_mem_valid_out and TestSequence_TestLongDouble_mem_valid_out;
-- NOTE: We could also solve this with a one-hot mux, but syntesis of a one-hot mux is more complex than a simple index mux. TestSequence_TestArray_valid <= TestSequence_TestArray_mem_valid_out(to_integer(unsigned(TestSequence_addr)));
-- Alternatively you could just OR all outputs, but that places the restriction that the data_out signal has to be (others => '0') when valid = '0'. TestSequence_TestArray_r <= TestSequence_TestArray_mem_data_out(to_integer(unsigned(TestSequence_addr)));
TestSequence_TestArray_r <= TestSequence_TestArray_mem_data_out(to_integer(unsigned(TestSequence_addr_latch))); TestSequence_TestArray_mem_data_in <= (others => TestSequence_TestArray_w);
TestSequence_TestArray_valid <= TestSequence_TestArray_mem_valid_out(to_integer(unsigned(TestSequence_addr_latch)));
TestSequence_TestChar_r <= TestSequence_TestChar_mem_data_out; TestSequence_TestChar_r <= TestSequence_TestChar_mem_data_out;
TestSequence_TestChar_mem_data_in <= TestSequence_TestChar_w;
TestSequence_TestWChar_r <= TestSequence_TestWChar_mem_data_out; TestSequence_TestWChar_r <= TestSequence_TestWChar_mem_data_out;
TestSequence_TestWChar_mem_data_in <= TestSequence_TestWChar_w;
TestSequence_TestLongLong_r <= TestSequence_TestLongLong_mem_data_out; TestSequence_TestLongLong_r <= TestSequence_TestLongLong_mem_data_out;
TestSequence_TestLongLong_mem_data_in <= TestSequence_TestLongLong_w;
TestSequence_TestLongDouble_r <= TestSequence_TestLongDouble_mem_data_out(CDR_LONG_DOUBLE_WIDTH-1 downto 0); TestSequence_TestLongDouble_r <= TestSequence_TestLongDouble_mem_data_out(CDR_LONG_DOUBLE_WIDTH-1 downto 0);
TestSequence_TestLongDouble_opt_r <= TestSequence_TestLongDouble_mem_data_out(CDR_LONG_DOUBLE_WIDTH); TestSequence_TestLongDouble_opt_r <= TestSequence_TestLongDouble_mem_data_out(CDR_LONG_DOUBLE_WIDTH);
TestSequence_TestLongDouble_mem_data_in <= TestSequence_TestLongDouble_opt_w & TestSequence_TestLongDouble_w;
TestMap_valid <= TestMap_key_mem_valid_out and TestMap_value_mem_valid_out; TestMap_valid <= TestMap_key_mem_valid_out and TestMap_value_mem_valid_out;
TestMap_key_r <= TestMap_key_mem_data_out; TestMap_key_r <= TestMap_key_mem_data_out;
TestMap_key_mem_data_in <= TestMap_key_w;
TestMap_value_r <= TestMap_value_mem_data_out; TestMap_value_r <= TestMap_value_mem_data_out;
TestMap_value_mem_data_in <= TestMap_value_w;
TestString_valid <= TestString_mem_valid_out; TestString_valid <= TestString_mem_valid_out;
TestString_r <= TestString_mem_data_out; TestString_r <= TestString_mem_data_out;
TestSequence_TestChar_mem_data_in <= TestSequence_TestChar_w;
TestSequence_TestWChar_mem_data_in <= TestSequence_TestWChar_w;
TestSequence_TestLongLong_mem_data_in <= TestSequence_TestLongLong_w;
TestSequence_TestLongDouble_mem_data_in <= TestSequence_TestLongDouble_opt_w & TestSequence_TestLongDouble_w;
TestSequence_TestArray_mem_data_in <= (others => TestSequence_TestArray_w);
TestMap_key_mem_data_in <= TestMap_key_w;
TestMap_value_mem_data_in <= TestMap_value_w;
TestString_mem_data_in <= TestString_w; TestString_mem_data_in <= TestString_w;
-- ###GENERATED END### -- ###GENERATED END###
@ -398,7 +395,6 @@ begin
-- ###GENERATED START### -- ###GENERATED START###
TestSequence_cnt_next <= TestSequence_cnt; TestSequence_cnt_next <= TestSequence_cnt;
TestSequence_TestArray_cnt_next <= TestSequence_TestArray_cnt; TestSequence_TestArray_cnt_next <= TestSequence_TestArray_cnt;
TestSequence_addr_latch_next <= TestSequence_addr_latch;
TestMap_cnt_next <= TestMap_cnt; TestMap_cnt_next <= TestMap_cnt;
TestString_cnt_next <= TestString_cnt; TestString_cnt_next <= TestString_cnt;
TestSequence_TestChar_mem_addr <= (others => '0'); TestSequence_TestChar_mem_addr <= (others => '0');
@ -491,10 +487,6 @@ begin
TestSequence_TestArray_mem_read(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren; TestSequence_TestArray_mem_read(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren;
TestSequence_TestArray_mem_valid_in(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren or TestSequence_TestArray_wen; TestSequence_TestArray_mem_valid_in(to_integer(unsigned(TestSequence_addr))) <= TestSequence_TestArray_ren or TestSequence_TestArray_wen;
TestSequence_TestArray_mem_ready_out <= (others => TestSequence_TestArray_ack); TestSequence_TestArray_mem_ready_out <= (others => TestSequence_TestArray_ack);
-- Latch Address for output
if (TestSequence_TestArray_ren = '1') then
TestSequence_addr_latch_next <= TestSequence_addr;
end if;
TestMap_ready <= TestMap_key_mem_ready_in and TestMap_value_mem_ready_in; TestMap_ready <= TestMap_key_mem_ready_in and TestMap_value_mem_ready_in;
TestMap_key_mem_addr <= TestMap_addr; TestMap_key_mem_addr <= TestMap_addr;
TestMap_key_mem_read <= TestMap_ren; TestMap_key_mem_read <= TestMap_ren;
@ -1153,7 +1145,6 @@ begin
TestSequence_TestArray_cnt <= 0; TestSequence_TestArray_cnt <= 0;
TestMap_cnt <= 0; TestMap_cnt <= 0;
TestString_cnt <= 0; TestString_cnt <= 0;
TestSequence_addr_latch <= (others => '0');
-- ###GENERATED END### -- ###GENERATED END###
else else
stage <= stage_next; stage <= stage_next;
@ -1169,7 +1160,6 @@ begin
TestSequence_TestArray_cnt <= TestSequence_TestArray_cnt_next; TestSequence_TestArray_cnt <= TestSequence_TestArray_cnt_next;
TestMap_cnt <= TestMap_cnt_next; TestMap_cnt <= TestMap_cnt_next;
TestString_cnt <= TestString_cnt_next; TestString_cnt <= TestString_cnt_next;
TestSequence_addr_latch <= TestSequence_addr_latch_next;
-- ###GENERATED END### -- ###GENERATED END###
end if; end if;
end if; end if;

85
src/ros2/ROS_IDL.txt
View File

@ -1,3 +1,16 @@
PUBLISH/SUBSCRIBE
#################
GENERAL
=======
The wrapper Entities that define and implement the ROS Publisher and Subscriber - named <TOPICNAME>_ros_pub
and <TOPICNAME>_ros_sub - are basically carbon copies of a normal <TYPENAME>_reader_interface and
<TYPENAME>_writer_interface, with the exception of hiding all the user facing DDS signals and being
responsible for interacting with the DDS entity directly.
All these changes are static for all ROS messages, and the actual message dependent VHDL generation is the
same, as the RTPS IDL interface generation.
SERVICE SERVICE
####### #######
@ -5,8 +18,11 @@ GENERAL
======= =======
The Service VHDL generation closely follows the general RTPS IDL interface generation. The Service VHDL generation closely follows the general RTPS IDL interface generation.
For each Service (i.e. .srv File) two files are generated, a server and a client file. For each Service (i.e. .srv File) two files are generated, a server (called <SERVICENAME>_ros_srv_server)
Each file contains both encoding and decoding functionality (similar to the key_holder files). and a client (called <SERVICENAME>_ros_srv_client) file.
Each file contains both encoding and decoding functionality (similar to the key_holder entities).
The files also contain extra logic that may be necessary to parse the SERVICE_OVERHEAD_BYTES that may be
present in the payload.
The code generation follows the normal code generation for encoding and decoding with following changes: The code generation follows the normal code generation for encoding and decoding with following changes:
* Encoding and Decoding stages get a "RQ_"or "RR_" prepended to the <NAME> to differentiate between * Encoding and Decoding stages get a "RQ_"or "RR_" prepended to the <NAME> to differentiate between
@ -24,3 +40,68 @@ The code generation follows the normal code generation for encoding and decoding
ACTION ACTION
###### ######
GENERAL
=======
ROS Actions are basically 3 ROS Services (GOAL, RESULT, CANCEL) and 2 ROS Topics (FEEDBACK, STATUS).
Of these the CANCEL Service, and the STATUS Topic are static for all ROS actions.
For each Action (i.e. .action File) 8 files are generated:
<ACTIONNAME>_ros_action_goal_srv_server (GOAL Service server)
<ACTIONNAME>_ros_action_goal_srv_client (GOAL Service client)
<ACTIONNAME>_ros_action_result_srv_server (RESULT Service server)
<ACTIONNAME>_ros_action_result_srv_client (RESULT Service client)
<ACTIONNAME>_ros_action_feedback_pub (FEEDBACK publisher)
<ACTIONNAME>_ros_action_feedback_sub (FEEDBACK subscriber)
<ACTIONNAME>_ros_action_server (Action server)
<ACTIONNAME>_ros_action_client (Action client)
The GOAL and RESULT serrvice files are similar to normal ROS Service files generation, with the addition
of static logic responsible for encoding/decoding static parts of the action messages.
Similarly the FEEDBACK publsiher/subscriber files is also similar to normal ROS publisher/subscriber files,
with the addition of static logic responsible for encoding/decoding static parts of the action messages.
ACTION SERVER
-------------
The ROS Action server is responsible for storing up to MAX_GOALS results.
This is achieved by storing results in memories of MAX_GOALS depth.
The ros_action_server entity contains the ROS action server logic, where as all the memories responsible
for storing the results reside in <ACTIONNAME>_ros_action_server.
The <ACTIONNAME>_ros_action_server contains a FSM responsible for fetching the requested RESULT on demand,
which is communcated from ros_action_server via the r_index, r_sel, and r_sel_ack signals.
In the IDLE stage, similar to the normal code generation, the memory signals are directly driven by the
user facing port signals, allowing the user to write the RESULT. The 'index_result' signal is used to index
the (outer) memories.
If r_sel is '1', the FETCH_RESULT stage is entered, which is responsible for fetching the correct RESULT.
The FETCH_RESULT stage is split into 2 sub-stages, a GET and a READ sub-stage. The GET sub-stage is waiting
for all memories to be ready (ANDing the ready_in signals) before toggling the valid_in signal to initiate
the memory read operation and proceeding to th next sub-stage. The READ sub-stage is waiting for all
memory read operations to finish (ANDing the valid_out signals) before proceeding to the next stage.
If the RESULT contains collections, they have to be manually pushed to the
<ACTIONNAME>_ros_action_result_srv_server. In that case the next stage is a stage called PUSH_<COLLECTIONNAME>,
which, similar to the normal code generation of a collection in a writer_interface, has 2 sub-stages
(GET, WRITE). The difference is that the second sub-stage (WRITE) is not only waiting on the valid_out
signal of the memory, but also on the respective ready port signal of the
<ACTIONNAME>_ros_action_result_srv_server.
Once the RESULT is ready and available on the <ACTIONNAME>_ros_action_result_srv_server, the PASSTHROUGH
stage is entered. In this stage the r_sel_ack signal is toggled, signaling to the ros_action_server that
it can continue its operations. The stage is held until the r_sel goes low, in which case the IDLE stage
is resumed.
If r_index is equal to MAX_GOALS, it is treated as a special value, in which the RESULT is empty/don't care.
If that is the case the PASSTHROUGH stage is taken directly (since we don't have to fetch any particular
result from the memories), and a empty RESULT is made available to the
<ACTIONNAME>_ros_action_result_srv_server.
If ENABLE_FEEDBACK is '0', no ROS publisher for the FEEDBACK is generated, and the FEEDBACK output ports
have to manually be set to default values.
NOTE: If the RESULT contains a collection, the read port signals of the
<ACTIONNAME>_ros_action_result_srv_server are not needed, and can be hardwired to defaults
(*_ren = '0', *_valid = open, *_ack = '1', *_r = open)
ACTION CLIENT
-------------
The <ACTIONNAME>_ros_action_client contains a FSM responsible for interfacing all the underlying service
clients and subscribers. Since this state machine is static, only the port connection of the
instantiated entities has to be generated.
Similarly to the action server, if ENABLE_FEEDBACK is '0', the FEEDBACK output ports have to manually be
set to default values.

751
src/ros2/TEMPLATE_ros_action_client.vhd Normal file
View File

@ -0,0 +1,751 @@
-- 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.math_pkg.all;
use work.rtps_package.all;
use work.ros_package.all;
use work.GoalStatus_package;
use work.GoalStatusArray_package;
use work.CancelGoal_package;
entity Fibonacci_ros_action_client is
generic (
ENABLE_FEEDBACK : std_logic := '1';
LITTLE_ENDIAN : std_logic := '0'
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
time : in ROS_TIME_TYPE;
-- *GOAL SERVICE*
-- DDS READER
g_start_r : out std_logic;
g_ack_r : in std_logic;
g_opcode_r : out DDS_READER_OPCODE_TYPE;
g_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
g_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
g_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
g_instance_handle_r : out INSTANCE_HANDLE_TYPE;
g_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
g_get_data_r : out std_logic;
g_done_r : in std_logic;
g_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
g_valid_in_r : in std_logic;
g_ready_in_r : out std_logic;
g_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
g_last_word_in_r : in std_logic;
g_sample_info_r : in SAMPLE_INFO_TYPE;
g_sample_info_valid_r : in std_logic;
g_sample_info_ack_r : out std_logic;
g_eoc_r : in std_logic;
g_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- DDS WRITER
g_start_w : out std_logic;
g_ack_w : in std_logic;
g_opcode_w : out DDS_WRITER_OPCODE_TYPE;
g_instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
g_source_ts_w : out TIME_TYPE;
g_max_wait_w : out DURATION_TYPE;
g_done_w : in std_logic;
g_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
g_instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
g_valid_out_w : out std_logic;
g_ready_out_w : in std_logic;
g_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
g_last_word_out_w : out std_logic;
g_valid_in_w : in std_logic;
g_ready_in_w : out std_logic;
g_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
g_last_word_in_w : in std_logic;
g_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *RESULT SERVICE*
-- DDS READER
r_start_r : out std_logic;
r_ack_r : in std_logic;
r_opcode_r : out DDS_READER_OPCODE_TYPE;
r_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
r_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
r_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
r_instance_handle_r : out INSTANCE_HANDLE_TYPE;
r_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
r_get_data_r : out std_logic;
r_done_r : in std_logic;
r_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
r_valid_in_r : in std_logic;
r_ready_in_r : out std_logic;
r_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
r_last_word_in_r : in std_logic;
r_sample_info_r : in SAMPLE_INFO_TYPE;
r_sample_info_valid_r : in std_logic;
r_sample_info_ack_r : out std_logic;
r_eoc_r : in std_logic;
r_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- DDS WRITER
r_start_w : out std_logic;
r_ack_w : in std_logic;
r_opcode_w : out DDS_WRITER_OPCODE_TYPE;
r_instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
r_source_ts_w : out TIME_TYPE;
r_max_wait_w : out DURATION_TYPE;
r_done_w : in std_logic;
r_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
r_instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
r_valid_out_w : out std_logic;
r_ready_out_w : in std_logic;
r_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
r_last_word_out_w : out std_logic;
r_valid_in_w : in std_logic;
r_ready_in_w : out std_logic;
r_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
r_last_word_in_w : in std_logic;
r_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *CANCEL SERVICE*
-- DDS READER
c_start_r : out std_logic;
c_ack_r : in std_logic;
c_opcode_r : out DDS_READER_OPCODE_TYPE;
c_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
c_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
c_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
c_instance_handle_r : out INSTANCE_HANDLE_TYPE;
c_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
c_get_data_r : out std_logic;
c_done_r : in std_logic;
c_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
c_valid_in_r : in std_logic;
c_ready_in_r : out std_logic;
c_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
c_last_word_in_r : in std_logic;
c_sample_info_r : in SAMPLE_INFO_TYPE;
c_sample_info_valid_r : in std_logic;
c_sample_info_ack_r : out std_logic;
c_eoc_r : in std_logic;
c_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- DDS WRITER
c_start_w : out std_logic;
c_ack_w : in std_logic;
c_opcode_w : out DDS_WRITER_OPCODE_TYPE;
c_instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
c_source_ts_w : out TIME_TYPE;
c_max_wait_w : out DURATION_TYPE;
c_done_w : in std_logic;
c_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
c_instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
c_valid_out_w : out std_logic;
c_ready_out_w : in std_logic;
c_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
c_last_word_out_w : out std_logic;
c_valid_in_w : in std_logic;
c_ready_in_w : out std_logic;
c_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
c_last_word_in_w : in std_logic;
c_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *FEEDBACK SUBSCRIBER*
f_start_dds : out std_logic;
f_ack_dds : in std_logic;
f_opcode_dds : out DDS_READER_OPCODE_TYPE;
f_instance_state_dds : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
f_view_state_dds : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
f_sample_state_dds : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
f_instance_handle_dds : out INSTANCE_HANDLE_TYPE;
f_max_samples_dds : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
f_get_data_dds : out std_logic;
f_done_dds : in std_logic;
f_return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
f_valid_in_dds : in std_logic;
f_ready_in_dds : out std_logic;
f_data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0);
f_last_word_in_dds : in std_logic;
f_sample_info_dds : in SAMPLE_INFO_TYPE;
f_sample_info_valid_dds : in std_logic;
f_sample_info_ack_dds : out std_logic;
f_eoc_dds : in std_logic;
f_status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *USER*
start : in std_logic;
opcode : in ROS_ACTION_OPCODE_TYPE;
ack : out std_logic;
sequence_id : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
service_info : out SERVICE_INFO_TYPE;
data_available_g : out std_logic;
data_available_r : out std_logic;
data_available_c : out std_logic;
data_available_f : out std_logic;
done : out std_logic;
return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
-- GOAL SIGNALS
goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0);
-- ###GENERATED START###
-- GOAL PORTS
-- ###GENERATED END###
goal_accepted : out std_logic;
goal_stamp : out ROS_TIME_TYPE;
-- RESULT SIGNALS
result_goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0);
result_status : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- ###GENERATED START###
-- RESULT PORTS
-- ###GENERATED END###
-- CANCEL
cancel_goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0);
cancel_stamp : in ROS_TIME_TYPE;
cancel_return_code : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
cancel_goals_canceling_len : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
cancel_goals_canceling_addr : in std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
cancel_goals_canceling_ready : out std_logic;
cancel_goals_canceling_ren : in std_logic;
cancel_goals_canceling_valid : out std_logic;
cancel_goals_canceling_ack : in std_logic;
cancel_goals_canceling_goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
cancel_goals_canceling_stamp : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
-- FEEDBACK SIGNALS
feedback_goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
-- ###GENERATED START###
-- FEEDBACK PORTS
-- ###GENERATED END###
);
end entity;
architecture arch of Fibonacci_ros_action_client is
--*****TYPE DECLARATION*****
type STAGE_TYPE is (IDLE,SEND_GOAL_REQUEST,TAKE_GOAL_RESPONSE,SEND_RESULT_REQUEST,TAKE_RESULT_RESPONSE,SEND_CANCEL_REQUEST,TAKE_CANCEL_RESPONSE,TAKE_FEEDBACK,RETURN_ROS);
--*****SIGNAL DECLARATION*****
signal stage, stage_next : STAGE_TYPE;
signal cnt, cnt_next : natural range 0 to 5;
signal g_start_user, r_start_user, c_start_user, g_ack_user, r_ack_user, c_ack_user, g_taken_user, r_taken_user, c_taken_user, g_done_user, r_done_user, c_done_user : std_logic;
signal g_opcode_user, r_opcode_user, c_opcode_user : ROS_SERVICE_OPCODE_TYPE;
signal g_service_info_user, r_service_info_user, c_service_info_user : SERVICE_INFO_TYPE;
signal g_sequence_id_user, r_sequence_id_user, c_sequence_id_user : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
signal g_return_code_user, r_return_code_user, c_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal f_start_user, f_ack_user, f_done_user, f_taken_user : std_logic;
signal f_opcode_user : ROS_TOPIC_OPCODE_TYPE;
signal f_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal f_message_info_user : MESSAGE_INFO_TYPE;
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal goal_stamp_sig : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
begin
goal_srv_client_inst : entity work.Fibonacci_ros_action_goal_srv_client(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_r => g_start_r,
ack_r => g_ack_r,
opcode_r => g_opcode_r,
instance_state_r => g_instance_state_r,
view_state_r => g_view_state_r,
sample_state_r => g_sample_state_r,
instance_handle_r => g_instance_handle_r,
max_samples_r => g_max_samples_r,
get_data_r => g_get_data_r,
done_r => g_done_r,
return_code_r => g_return_code_r,
valid_in_r => g_valid_in_r,
ready_in_r => g_ready_in_r,
data_in_r => g_data_in_r,
last_word_in_r => g_last_word_in_r,
sample_info_r => g_sample_info_r,
sample_info_valid_r => g_sample_info_valid_r,
sample_info_ack_r => g_sample_info_ack_r,
eoc_r => g_eoc_r,
status_r => g_status_r,
start_w => g_start_w,
ack_w => g_ack_w,
opcode_w => g_opcode_w,
instance_handle_out_w => g_instance_handle_out_w,
source_ts_w => g_source_ts_w,
max_wait_w => g_max_wait_w,
done_w => g_done_w,
return_code_w => g_return_code_w,
instance_handle_in_w => g_instance_handle_in_w,
valid_out_w => g_valid_out_w,
ready_out_w => g_ready_out_w,
data_out_w => g_data_out_w,
last_word_out_w => g_last_word_out_w,
valid_in_w => g_valid_in_w,
ready_in_w => g_ready_in_w,
data_in_w => g_data_in_w,
last_word_in_w => g_last_word_in_w,
status_w => g_status_w,
start_user => g_start_user,
ack_user => g_ack_user,
opcode_user => g_opcode_user,
done_user => g_done_user,
return_code_user => g_return_code_user,
service_info_user => g_service_info_user,
sequence_id_user => g_sequence_id_user,
data_available_user => data_available_g,
taken_user => g_taken_user,
goal_id => goal_id,
-- ###GENERATED START###
-- GOAL PORT CONNECTIONS
-- ###GENERATED END###
accepted => goal_accepted,
stamp => goal_stamp_sig
);
goal_stamp <= to_ROS_TIME(goal_stamp_sig);
result_srv_client_inst : entity work.Fibonacci_ros_action_result_srv_client(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_r => r_start_r,
ack_r => r_ack_r,
opcode_r => r_opcode_r,
instance_state_r => r_instance_state_r,
view_state_r => r_view_state_r,
sample_state_r => r_sample_state_r,
instance_handle_r => r_instance_handle_r,
max_samples_r => r_max_samples_r,
get_data_r => r_get_data_r,
done_r => r_done_r,
return_code_r => r_return_code_r,
valid_in_r => r_valid_in_r,
ready_in_r => r_ready_in_r,
data_in_r => r_data_in_r,
last_word_in_r => r_last_word_in_r,
sample_info_r => r_sample_info_r,
sample_info_valid_r => r_sample_info_valid_r,
sample_info_ack_r => r_sample_info_ack_r,
eoc_r => r_eoc_r,
status_r => r_status_r,
start_w => r_start_w,
ack_w => r_ack_w,
opcode_w => r_opcode_w,
instance_handle_out_w => r_instance_handle_out_w,
source_ts_w => r_source_ts_w,
max_wait_w => r_max_wait_w,
done_w => r_done_w,
return_code_w => r_return_code_w,
instance_handle_in_w => r_instance_handle_in_w,
valid_out_w => r_valid_out_w,
ready_out_w => r_ready_out_w,
data_out_w => r_data_out_w,
last_word_out_w => r_last_word_out_w,
valid_in_w => r_valid_in_w,
ready_in_w => r_ready_in_w,
data_in_w => r_data_in_w,
last_word_in_w => r_last_word_in_w,
status_w => r_status_w,
start_user => r_start_user,
ack_user => r_ack_user,
opcode_user => r_opcode_user,
done_user => r_done_user,
return_code_user => r_return_code_user,
service_info_user => r_service_info_user,
sequence_id_user => r_sequence_id_user,
data_available_user => data_available_r,
taken_user => r_taken_user,
goal_id => result_goal_id,
status => result_status,
-- ###GENERATED START###
-- RESULT PORT CONNECTIONS
-- ###GENERATED END###
);
cancel_srv_client_inst : entity work.CancelGoal_ros_srv_client(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_r => c_start_r,
ack_r => c_ack_r,
opcode_r => c_opcode_r,
instance_state_r => c_instance_state_r,
view_state_r => c_view_state_r,
sample_state_r => c_sample_state_r,
instance_handle_r => c_instance_handle_r,
max_samples_r => c_max_samples_r,
get_data_r => c_get_data_r,
done_r => c_done_r,
return_code_r => c_return_code_r,
valid_in_r => c_valid_in_r,
ready_in_r => c_ready_in_r,
data_in_r => c_data_in_r,
last_word_in_r => c_last_word_in_r,
sample_info_r => c_sample_info_r,
sample_info_valid_r => c_sample_info_valid_r,
sample_info_ack_r => c_sample_info_ack_r,
eoc_r => c_eoc_r,
status_r => c_status_r,
start_w => c_start_w,
ack_w => c_ack_w,
opcode_w => c_opcode_w,
instance_handle_out_w => c_instance_handle_out_w,
source_ts_w => c_source_ts_w,
max_wait_w => c_max_wait_w,
done_w => c_done_w,
return_code_w => c_return_code_w,
instance_handle_in_w => c_instance_handle_in_w,
valid_out_w => c_valid_out_w,
ready_out_w => c_ready_out_w,
data_out_w => c_data_out_w,
last_word_out_w => c_last_word_out_w,
valid_in_w => c_valid_in_w,
ready_in_w => c_ready_in_w,
data_in_w => c_data_in_w,
last_word_in_w => c_last_word_in_w,
status_w => c_status_w,
start_user => c_start_user,
ack_user => c_ack_user,
opcode_user => c_opcode_user,
done_user => c_done_user,
return_code_user => c_return_code_user,
service_info_user => c_service_info_user,
sequence_id_user => c_sequence_id_user,
data_available_user => data_available_c,
taken_user => c_taken_user,
goal_info_goal_id => cancel_goal_id,
goal_info_stamp => std_logic_vector(to_unsigned(cancel_stamp)),
return_code => cancel_return_code,
goals_canceling_len => cancel_goals_canceling_len,
goals_canceling_addr => cancel_goals_canceling_addr,
goals_canceling_ready => cancel_goals_canceling_ready,
goals_canceling_ren => cancel_goals_canceling_ren,
goals_canceling_valid => cancel_goals_canceling_valid,
goals_canceling_ack => cancel_goals_canceling_ack,
goals_canceling_goal_id => cancel_goals_canceling_goal_id,
goals_canceling_stamp => cancel_goals_canceling_stamp
);
feedback_gen : if ENABLE_FEEDBACK = '1' generate
feedback_sub_inst : entity work.Fibonacci_ros_action_feedback_sub(arch)
port map (
clk => clk,
reset => reset,
start_dds => f_start_dds,
ack_dds => f_ack_dds,
opcode_dds => f_opcode_dds,
instance_state_dds => f_instance_state_dds,
view_state_dds => f_view_state_dds,
sample_state_dds => f_sample_state_dds,
instance_handle_dds => f_instance_handle_dds,
max_samples_dds => f_max_samples_dds,
get_data_dds => f_get_data_dds,
done_dds => f_done_dds,
return_code_dds => f_return_code_dds,
valid_in_dds => f_valid_in_dds,
ready_in_dds => f_ready_in_dds,
data_in_dds => f_data_in_dds,
last_word_in_dds => f_last_word_in_dds,
sample_info_dds => f_sample_info_dds,
sample_info_valid_dds => f_sample_info_valid_dds,
sample_info_ack_dds => f_sample_info_ack_dds,
eoc_dds => f_eoc_dds,
status_dds => f_status_dds,
start_user => f_start_user,
opcode_user => f_opcode_user,
ack_user => f_ack_user,
done_user => f_done_user,
return_code_user => f_return_code_user,
message_info_user => f_message_info_user,
taken_user => f_taken_user,
data_available_user => data_available_f,
-- ###GENERATED START###
-- FEEDBACK PORT CONNECTIONS
-- ###GENERATED END###
);
else generate
f_start_dds <= '0';
f_opcode_dds <= NOP;
f_instance_state_dds <= ANY_INSTANCE_STATE;
f_view_state_dds <= ANY_VIEW_STATE;
f_sample_state_dds <= ANY_SAMPLE_STATE;
f_instance_handle_dds <= HANDLE_NIL;
f_max_samples_dds <= (others => '0');
f_get_data_dds <= '0';
f_ready_in_dds <= '0';
f_sample_info_ack_dds <= '0';
f_ack_user <= '0';
f_done_user <= '0';
f_return_code_user <= ROS_RET_OK;
data_available_f <= '0';
f_message_info_user <= EMPTY_MESSAGE_INFO;
f_taken_user <= '0';
feedback_goal_id <= (others => '0');
-- ###GENERATED START###
-- DEFAULT FEEDBACK PORT VALUES
-- ###GENERATED END###
end generate;
main_prc : process(all)
begin
-- DEFAULT
stage_next <= stage;
cnt_next <= cnt;
return_code_latch_next <= return_code_latch;
-- DEFAULT Unregistered
f_start_user <= '0';
f_opcode_user <= NOP;
g_start_user <= '0';
r_opcode_user <= NOP;
r_start_user <= '0';
g_opcode_user <= NOP;
c_start_user <= '0';
c_opcode_user <= NOP;
sequence_id <= (others => '0');
service_info <= EMPTY_SERVICE_INFO;
ack <= '0';
done <= '0';
return_code <= ROS_RET_OK;
case (stage) is
when IDLE =>
if (start = '1') then
ack <= '1';
case (opcode) is
when SEND_GOAL_REQUEST =>
stage_next <= SEND_GOAL_REQUEST;
cnt_next <= 0;
when TAKE_GOAL_RESPONSE =>
stage_next <= TAKE_GOAL_RESPONSE;
cnt_next <= 0;
when SEND_RESULT_REQUEST =>
stage_next <= SEND_RESULT_REQUEST;
cnt_next <= 0;
when TAKE_RESULT_RESPONSE =>
stage_next <= TAKE_RESULT_RESPONSE;
cnt_next <= 0;
when SEND_CANCEL_REQUEST =>
stage_next <= SEND_CANCEL_REQUEST;
cnt_next <= 0;
when TAKE_CANCEL_RESPONSE =>
stage_next <= TAKE_CANCEL_RESPONSE;
cnt_next <= 0;
when TAKE_FEEDBACK =>
-- SYNTHESIS GUARD
if (ENABLE_FEEDBACK = '1') then
stage_next <= TAKE_FEEDBACK;
cnt_next <= 0;
else
return_code_latch_next <= ROS_RET_UNSUPPORTED;
stage_next <= RETURN_ROS;
end if;
when others =>
return_code_latch_next <= ROS_RET_UNSUPPORTED;
stage_next <= RETURN_ROS;
end case;
end if;
when SEND_GOAL_REQUEST =>
case (cnt) is
-- Take
when 0 =>
g_start_user <= '1';
g_opcode_user <= SEND_REQUEST;
if (g_ack_user = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Subscriber
when 1 =>
sequence_id <= g_sequence_id_user;
done <= g_done_user;
return_code <= g_return_code_user;
if (g_done_user = '1') then
-- DONE
stage_next <= IDLE;
end if;
when others =>
null;
end case;
when TAKE_GOAL_RESPONSE =>
case (cnt) is
-- Take
when 0 =>
g_start_user <= '1';
g_opcode_user <= TAKE_RESPONSE;
if (g_ack_user = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Subscriber
when 1 =>
service_info <= g_service_info_user;
return_code <= g_return_code_user;
if (g_done_user = '1') then
if (g_taken_user = '1') then
done <= g_done_user;
stage_next <= IDLE;
else
return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED;
stage_next <= RETURN_ROS;
end if;
end if;
when others =>
null;
end case;
when SEND_RESULT_REQUEST =>
case (cnt) is
-- Take
when 0 =>
r_start_user <= '1';
r_opcode_user <= SEND_REQUEST;
if (r_ack_user = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Subscriber
when 1 =>
sequence_id <= r_sequence_id_user;
done <= r_done_user;
return_code <= r_return_code_user;
if (r_done_user = '1') then
-- DONE
stage_next <= IDLE;
end if;
when others =>
null;
end case;
when TAKE_RESULT_RESPONSE =>
case (cnt) is
-- Take
when 0 =>
r_start_user <= '1';
r_opcode_user <= TAKE_RESPONSE;
if (r_ack_user = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Subscriber
when 1 =>
service_info <= r_service_info_user;
return_code <= r_return_code_user;
if (r_done_user = '1') then
if (r_taken_user = '1') then
done <= r_done_user;
stage_next <= IDLE;
else
return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED;
stage_next <= RETURN_ROS;
end if;
end if;
when others =>
null;
end case;
when SEND_CANCEL_REQUEST =>
case (cnt) is
-- Take
when 0 =>
c_start_user <= '1';
c_opcode_user <= SEND_REQUEST;
if (c_ack_user = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Subscriber
when 1 =>
sequence_id <= c_sequence_id_user;
done <= c_done_user;
return_code <= c_return_code_user;
if (c_done_user = '1') then
-- DONE
stage_next <= IDLE;
end if;
when others =>
null;
end case;
when TAKE_CANCEL_RESPONSE =>
case (cnt) is
-- Take
when 0 =>
c_start_user <= '1';
c_opcode_user <= TAKE_RESPONSE;
if (c_ack_user = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Subscriber
when 1 =>
service_info <= c_service_info_user;
return_code <= c_return_code_user;
if (c_done_user = '1') then
if (c_taken_user = '1') then
done <= c_done_user;
stage_next <= IDLE;
else
return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED;
stage_next <= RETURN_ROS;
end if;
end if;
when others =>
null;
end case;
when TAKE_FEEDBACK =>
case (cnt) is
-- Take
when 0 =>
f_start_user <= '1';
f_opcode_user <= TAKE;
if (f_ack_user = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Subscriber
when 1 =>
if (f_done_user = '1') then
return_code <= f_return_code_user;
if (f_taken_user = '1') then
done <= f_done_user;
-- DONE
stage_next <= IDLE;
else
return_code_latch_next <= ROS_RET_ACTION_CLIENT_TAKE_FAILED;
stage_next <= RETURN_ROS;
end if;
end if;
when others =>
null;
end case;
when RETURN_ROS =>
done <= '1';
return_code <= return_code_latch;
-- DONE
stage_next <= IDLE;
end case;
end process;
sync_prc : process(clk)
begin
if rising_edge(clk) then
if (reset = '1') then
stage <= IDLE;
cnt <= 0;
return_code_latch <= ROS_RET_OK;
else
stage <= stage_next;
cnt <= cnt_next;
return_code_latch <= return_code_latch_next;
end if;
end if;
end process;
end architecture;

739
src/ros2/TEMPLATE_ros_action_goal_srv_client.vhd Normal file
View File

@ -0,0 +1,739 @@
-- 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;
entity TEMPLATE_ros_action_goal_srv_client is
generic (
LITTLE_ENDIAN : std_logic := '0'
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
-- FROM DDS READER
start_r : out std_logic;
ack_r : in std_logic;
opcode_r : out DDS_READER_OPCODE_TYPE;
instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
instance_handle_r : out INSTANCE_HANDLE_TYPE;
max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
get_data_r : out std_logic;
done_r : in std_logic;
return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
valid_in_r : in std_logic;
ready_in_r : out std_logic;
data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_r : in std_logic;
sample_info_r : in SAMPLE_INFO_TYPE;
sample_info_valid_r : in std_logic;
sample_info_ack_r : out std_logic;
eoc_r : in std_logic;
status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- FROM DDS WRITER
start_w : out std_logic;
ack_w : in std_logic;
opcode_w : out DDS_WRITER_OPCODE_TYPE;
instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
source_ts_w : out TIME_TYPE;
max_wait_w : out DURATION_TYPE;
done_w : in std_logic;
return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
valid_out_w : out std_logic;
ready_out_w : in std_logic;
data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_out_w : out std_logic;
valid_in_w : in std_logic;
ready_in_w : out std_logic;
data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_w : in std_logic;
status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- TO USER
start_user : in std_logic;
ack_user : out std_logic;
opcode_user : in ROS_SERVICE_OPCODE_TYPE;
service_info_user : out SERVICE_INFO_TYPE;
sequence_id_user : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
data_available_user : out std_logic;
taken_user : out std_logic;
-- REQUEST
goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0);
-- ###GENERATED START###
-- GOAL PORTS
-- ###GENERATED END###
-- RESPONSE
accepted : out std_logic;
stamp : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
done_user : out std_logic;
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)
);
end entity;
architecture arch of TEMPLATE_ros_action_goal_srv_client is
--*****TYPE DECLARATION*****
-- FSM states. Explained below in detail
type STAGE_TYPE is (IDLE,RETURN_ROS,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_WRITER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_IN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_OUT_STREAM,ENCODE_PAYLOAD);
type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RR_ACCEPTED,GET_RR_STAMP);
-- ###GENERATED START###
type ENCODE_STAGE_TYPE is (WRITE_RID_WGUID,WRITE_RID_SN,WRITE_RQ_GOAL_ID,TODO);
-- TYPE DECLARATIONS
-- ###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 dw_latch, dw_latch_next : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
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 data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
signal abort_mem : std_logic;
signal ready_in_r_sig : std_logic;
signal taken_sig, taken_sig_next : std_logic;
signal service_info_sig, service_info_sig_next : SERVICE_INFO_TYPE;
signal sequence_id_sig, sequence_id_sig_next : unsigned(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
signal finalize_payload, finalize_payload_next : std_logic;
signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE;
signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE;
signal return_stage, return_stage_next : DECODE_STAGE_TYPE;
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal data_available_sig, data_available_sig_next : std_logic;
signal uuid_cnt, uuid_cnt_next : natural range 0 to G_RQ_GOAL_ID_MAX_DEPTH-1;
signal accepted_latch, accepted_latch_next : std_logic;
signal stamp_latch, stamp_latch_next : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
-- ###GENERATED START###
-- SIGNAL DECLARATIONS
-- ###GENERATED END###
--*****ALIAS DECLARATION*****
alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(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_r(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###
-- MEMORY INSTANTIATIONS
-- ###GENERATED END###
instance_state_r <= ANY_INSTANCE_STATE;
view_state_r <= ANY_VIEW_STATE;
sample_state_r <= ANY_SAMPLE_STATE;
instance_handle_r <= HANDLE_NIL;
max_samples_r <= (others => '0');
instance_handle_in_w <= HANDLE_NIL;
source_ts_w <= TIME_INVALID;
max_wait_w <= DURATION_ZERO;
ready_in_w <= '0'; -- DDS Writer Input is unused
taken_user <= taken_sig;
ready_in_r <= ready_in_r_sig;
service_info_user <= service_info_sig;
sequence_id_user <= std_logic_vector(sequence_id_sig);
data_available_user <= data_available_sig;
accepted <= accepted_latch;
stamp <= stamp_latch;
-- ###GENERATED START###
-- PORT SIGNAL CONNECTIONS
-- ###GENERATED END###
main_prc : process (all)
variable tmp_length : unsigned(WORD_WIDTH-1 downto 0);
begin
-- DEFAULT
stage_next <= stage;
encode_stage_next <= encode_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;
data_out_latch_next <= data_out_latch;
finalize_payload_next <= finalize_payload;
optional_next <= optional;
taken_sig_next <= taken_sig;
data_in_latch_next <= data_in_latch;
align_op_next <= align_op;
return_code_latch_next <= return_code_latch;
service_info_sig_next <= service_info_sig;
dw_latch_next <= dw_latch;
sequence_id_sig_next <= sequence_id_sig;
data_available_sig_next <= data_available_sig;
ready_in_r_sig <= '0';
abort_mem <= '0';
ack_user <= '0';
sample_info_ack_r <= '0';
get_data_r <= '0';
start_r <= '0';
opcode_r <= NOP;
start_w <= '0';
opcode_w <= NOP;
valid_out_w <= '0';
last_word_out_w <= '0';
done_user <= '0';
return_code_user <= ROS_RET_OK;
data_out_w <= (others => '0');
uuid_cnt_next <= uuid_cnt;
accepted_latch_next <= accepted_latch;
stamp_latch_next <= stamp_latch;
-- ###GENERATED START###
-- DEFAULT SIGNAL ASSIGNMENTS
-- ###GENERATED END###
-- Last Word Latch Setter
if (last_word_in_r = '1') then
last_word_in_latch_next <= '1';
end if;
-- Data Available Setter
if (check_mask(status_r, 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 SEND_REQUEST =>
stage_next <= INITIATE_WRITE;
-- Increment Sequence ID
sequence_id_sig_next <= sequence_id_sig + 1;
when TAKE_RESPONSE =>
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###
-- MEMORY SIGNAL CONNECTIONS
-- ###GENERATED END###
end if;
when RETURN_ROS =>
done_user <= '1';
return_code_user <= return_code_latch;
-- DONE
stage_next <= IDLE;
when INITIATE_READ =>
start_r <= '1';
opcode_r <= TAKE_NEXT_SAMPLE;
if (ack_r = '1') then
stage_next <= WAIT_FOR_READER;
end if;
when WAIT_FOR_READER =>
if (done_r = '1') then
case (return_code_r) 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 (sample_info_valid_r = '1') then
sample_info_ack_r <= '1';
-- Meta Sample
if (sample_info_r.valid_data = '0') then
-- Ignore and read Next Sample
stage_next <= INITIATE_READ;
else
get_data_r <= '1';
stage_next <= GET_PAYLOAD_HEADER;
service_info_sig_next.received_timestamp <= TIME_INVALID;
service_info_sig_next.source_timestamp <= sample_info_r.source_timestamp;
end if;
end if;
when GET_PAYLOAD_HEADER =>
-- TODO: Latch Offset from Options Field?
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
case (representation_id) is
when CDR_BE =>
endian_flag_next <= '0';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
-- Initial Fetch
when CDR_LE =>
endian_flag_next <= '1';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
when others =>
-- Unknown Payload Encoding
stage_next <= SKIP_PAYLOAD;
decode_error_latch_next <= '1';
end case;
end if;
when FETCH =>
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
data_in_latch_next <= data_in_r;
-- Alignment Operation in progress
if (align_op = '1') then
stage_next <= ALIGN_IN_STREAM;
-- Reset
align_op_next <= '0';
else
stage_next <= DECODE_PAYLOAD;
end if;
end if;
when ALIGN_IN_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
-- NOTE: The Cyclone DDS implementation uses a custom request header that is pre-pended to the actual service request/response.
-- It is defined as follows:
-- struct cdds_request_header_t{
-- uint64_t guid;
-- int64_t seq;
-- };
-- 'seq' is set by a counter that is incremented on each "send_request".
-- 'guid' is set to the publication handle of the request writer of the service client.
-- Note that the publication handle is useless for the server, since it is only meanigful localy (i.e. only the client can do something with it)
-- Nevertheless the same 'guid' has to be returned to the client.
when GET_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.writer_guid(0) <= get_sub_vector(endian_swap(endian_flag, dw_latch),0,WORD_WIDTH,TRUE);
service_info_sig_next.request_id.writer_guid(1) <= get_sub_vector(endian_swap(endian_flag, dw_latch),1,WORD_WIDTH,TRUE);
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RID_SN;
cnt_next <= 0;
when others =>
null;
end case;
end if;
when GET_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.sequence_number <= to_double_word(unsigned(endian_swap(endian_flag, dw_latch)));
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RR_ACCEPTED;
when others =>
null;
end case;
end if;
when GET_RR_ACCEPTED =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_IN_STREAM;
else
accepted_latch_next <= get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_BOOLEAN_WIDTH, TRUE)(0);
align_offset_next <= align_offset + 1;
decode_stage_next <= GET_RR_STAMP;
cnt_next <= 0;
-- Need to fetch next Word
if(align_offset(1 downto 0) = "11") then
stage_next <= FETCH;
end if;
end if;
when GET_RR_STAMP =>
-- We intepret the TIME as a double word for efficiency
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next <= write_sub_vector(dw_latch, endian_swap(endian_flag, data_in_latch), 0, TRUE);
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next <= write_sub_vector(dw_latch, endian_swap(endian_flag, data_in_latch), 1, TRUE);
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
stamp_latch_next <= dw_latch;
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
-- DONE
stage_next <= SKIP_PAYLOAD;
when others =>
null;
end case;
end if;
when GET_OPTIONAL_HEADER =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_IN_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_r) = (data_in_r'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') then
-- Skip Read
ready_in_r_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;
when INITIATE_WRITE =>
start_w <= '1';
opcode_w <= WRITE;
if (ack_w = '1') then
stage_next <= WRITE_PAYLOAD_HEADER;
end if;
when WRITE_PAYLOAD_HEADER =>
valid_out_w <= '1';
if (LITTLE_ENDIAN = '0') then
data_out_w <= CDR_BE & x"0000";
else
data_out_w <= CDR_LE & x"0000";
end if;
-- Output Guard
if (ready_out_w = '1') then
stage_next <= ENCODE_PAYLOAD;
-- Reset
align_offset_next <= (others => '0');
data_out_latch_next <= (others => '0');
encode_stage_next <= WRITE_RID_WGUID;
cnt_next <= 0;
end if;
when PUSH =>
-- Mark Last Word
if (finalize_payload = '1') then
last_word_out_w <= '1';
end if;
valid_out_w <= '1';
data_out_w <= data_out_latch;
-- Output Guard
if (ready_out_w = '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_OUT_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_OUT_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
when WRITE_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= (others => '0');
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= (others => '0');
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RID_SN;
cnt_next <= 0;
when others =>
end case;
end if;
when WRITE_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(sequence_id_sig)), 0, WORD_WIDTH, TRUE);
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(sequence_id_sig)), 1, WORD_WIDTH, TRUE);
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RQ_GOAL_ID;
when others =>
end case;
end if;
when WRITE_RQ_GOAL_ID =>
-- Special Encoding for effieciency (Prevent having to define memory for UUID)
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_OUT_STREAM;
else
data_out_latch_next <= write_sub_vector(data_out_latch, get_sub_vector(goal_id,uuid_cnt,CDR_INT8_WIDTH,TRUE), to_integer(align_offset(1 downto 0)), TRUE);
align_offset_next <= align_offset + 1;
if (uuid_cnt = G_RQ_GOAL_ID_MAX_DEPTH-1) then
-- ###GENERATED START###
encode_stage_next <= TODO;
-- ###GENERATED END###
uuid_cnt_next <= 0; -- Post-Reset
else
uuid_cnt_next <= uuid_cnt + 1;
end if;
-- Need to fetch next Word
if(align_offset(1 downto 0) = "11") then
stage_next <= PUSH;
end if;
end if;
-- ###GENERATED START###
when TODO =>
-- ###GENERATED END###
when others =>
null;
end case;
when WAIT_FOR_WRITER =>
if (done_w = '1') then
case (return_code_w) 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;
when others =>
null;
end case;
-- OVERREAD GUARD
-- Attempted read on empty input
if (last_word_in_latch = '1' and last_word_in_r = '0' and ready_in_r_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;
encode_stage <= WRITE_RID_WGUID;
decode_stage <= GET_RID_WGUID;
return_stage <= GET_RID_WGUID;
target_align <= ALIGN_1;
cnt <= 0;
endian_flag <= '0';
last_word_in_latch <= '0';
decode_error_latch <= '0';
optional <= '0';
taken_sig <= '0';
align_op <= '0';
finalize_payload <= '0';
data_available_sig <= '0';
align_offset <= (others => '0');
data_in_latch <= (others => '0');
data_out_latch <= (others => '0');
dw_latch <= (others => '0');
sequence_id_sig <= (others => '0');
return_code_latch <= ROS_RET_OK;
service_info_sig <= EMPTY_SERVICE_INFO;
uuid_cnt <= 0;
accepted_latch <= '0';
stamp_latch <= (others => '0');
-- ###GENERATED START###
-- RESET SYNC SIGNAL VALUE
-- ###GENERATED END###
else
stage <= stage_next;
encode_stage <= encode_stage_next;
decode_stage <= decode_stage_next;
return_stage <= return_stage_next;
target_align <= target_align_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;
finalize_payload <= finalize_payload_next;
data_available_sig <= data_available_sig_next;
align_offset <= align_offset_next;
data_in_latch <= data_in_latch_next;
data_out_latch <= data_out_latch_next;
dw_latch <= dw_latch_next;
sequence_id_sig <= sequence_id_sig_next;
return_code_latch <= return_code_latch_next;
service_info_sig <= service_info_sig_next;
uuid_cnt <= uuid_cnt_next;
accepted_latch <= accepted_latch_next;
stamp_latch <= stamp_latch_next;
-- ###GENERATED START###
-- SYNC SIGNALS
-- ###GENERATED END###
end if;
end if;
end process;
end architecture;

727
src/ros2/TEMPLATE_ros_action_goal_srv_server.vhd Normal file
View File

@ -0,0 +1,727 @@
-- 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;
entity TEMPLATE_ros_action_goal_srv_server is
generic (
LITTLE_ENDIAN : std_logic := '0'
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
-- FROM DDS READER
start_r : out std_logic;
ack_r : in std_logic;
opcode_r : out DDS_READER_OPCODE_TYPE;
instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
instance_handle_r : out INSTANCE_HANDLE_TYPE;
max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
get_data_r : out std_logic;
done_r : in std_logic;
return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
valid_in_r : in std_logic;
ready_in_r : out std_logic;
data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_r : in std_logic;
sample_info_r : in SAMPLE_INFO_TYPE;
sample_info_valid_r : in std_logic;
sample_info_ack_r : out std_logic;
eoc_r : in std_logic;
status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- FROM DDS WRITER
start_w : out std_logic;
ack_w : in std_logic;
opcode_w : out DDS_WRITER_OPCODE_TYPE;
instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
source_ts_w : out TIME_TYPE;
max_wait_w : out DURATION_TYPE;
done_w : in std_logic;
return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
valid_out_w : out std_logic;
ready_out_w : in std_logic;
data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_out_w : out std_logic;
valid_in_w : in std_logic;
ready_in_w : out std_logic;
data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_w : in std_logic;
status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- TO USER
start_user : in std_logic;
ack_user : out std_logic;
opcode_user : in ROS_SERVICE_OPCODE_TYPE;
service_info_user : out SERVICE_INFO_TYPE;
request_id_user : in REQUEST_ID_TYPE;
data_available_user : out std_logic;
taken_user : out std_logic;
-- REQUEST
goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
-- ###GENERATED START###
-- GOAL PORTS
-- ###GENERATED END###
-- RESPONSE
accepted : in std_logic;
stamp : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
done_user : out std_logic;
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)
);
end entity;
architecture arch of TEMPLATE_ros_action_goal_srv_server is
--*****TYPE DECLARATION*****
-- FSM states. Explained below in detail
type STAGE_TYPE is (IDLE,RETURN_ROS,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_WRITER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_IN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_OUT_STREAM,ENCODE_PAYLOAD);
type ENCODE_STAGE_TYPE is (WRITE_RID_WGUID,WRITE_RID_SN,WRITE_RR_ACCEPTED,WRITE_RR_STAMP);
-- ###GENERATED START###
type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RQ_GOAL_ID,TODO);
-- TYPE DECLARATIONS
-- ###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 dw_latch, dw_latch_next : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
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 data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
signal abort_mem : std_logic;
signal ready_in_r_sig : std_logic;
signal taken_sig, taken_sig_next : std_logic;
signal service_info_sig, service_info_sig_next : SERVICE_INFO_TYPE;
signal finalize_payload, finalize_payload_next : std_logic;
signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE;
signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE;
signal return_stage, return_stage_next : DECODE_STAGE_TYPE;
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal data_available_sig, data_available_sig_next : std_logic;
signal uuid_cnt, uuid_cnt_next : natural range 0 to G_RQ_GOAL_ID_MAX_DEPTH-1;
signal goal_id_latch, goal_id_latch_next : std_logic_vector(UUID_WIDTH-1 downto 0);
-- ###GENERATED START###
-- SIGNAL DECLARATIONS
-- ###GENERATED END###
--*****ALIAS DECLARATION*****
alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(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_r(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###
-- MEMORY INSTANTIATIONS
-- ###GENERATED END###
instance_state_r <= ANY_INSTANCE_STATE;
view_state_r <= ANY_VIEW_STATE;
sample_state_r <= ANY_SAMPLE_STATE;
instance_handle_r <= HANDLE_NIL;
max_samples_r <= (others => '0');
instance_handle_in_w <= HANDLE_NIL;
source_ts_w <= TIME_INVALID;
max_wait_w <= DURATION_ZERO;
ready_in_w <= '0'; -- DDS Writer Input is unused
taken_user <= taken_sig;
ready_in_r <= ready_in_r_sig;
service_info_user <= service_info_sig;
data_available_user <= data_available_sig;
goal_id <= goal_id_latch;
-- ###GENERATED START###
-- PORT SIGNAL CONNECTIONS
-- ###GENERATED END###
main_prc : process (all)
variable tmp_length : unsigned(WORD_WIDTH-1 downto 0);
begin
-- DEFAULT
stage_next <= stage;
encode_stage_next <= encode_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;
data_out_latch_next <= data_out_latch;
finalize_payload_next <= finalize_payload;
optional_next <= optional;
taken_sig_next <= taken_sig;
data_in_latch_next <= data_in_latch;
align_op_next <= align_op;
return_code_latch_next <= return_code_latch;
service_info_sig_next <= service_info_sig;
dw_latch_next <= dw_latch;
data_available_sig_next <= data_available_sig;
ready_in_r_sig <= '0';
abort_mem <= '0';
ack_user <= '0';
sample_info_ack_r <= '0';
get_data_r <= '0';
start_r <= '0';
opcode_r <= NOP;
start_w <= '0';
opcode_w <= NOP;
valid_out_w <= '0';
last_word_out_w <= '0';
done_user <= '0';
return_code_user <= ROS_RET_OK;
data_out_w <= (others => '0');
uuid_cnt_next <= uuid_cnt;
goal_id_latch_next <= goal_id_latch;
-- ###GENERATED START###
-- DEFAULT SIGNAL ASSIGNMENTS
-- ###GENERATED END###
-- Last Word Latch Setter
if (last_word_in_r = '1') then
last_word_in_latch_next <= '1';
end if;
-- Data Available Setter
if (check_mask(status_r, 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_REQUEST =>
stage_next <= INITIATE_READ;
when SEND_RESPONSE =>
stage_next <= INITIATE_WRITE;
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###
-- MEMORY SIGNAL CONNECTIONS
-- ###GENERATED END###
end if;
when RETURN_ROS =>
done_user <= '1';
return_code_user <= return_code_latch;
-- DONE
stage_next <= IDLE;
when INITIATE_READ =>
start_r <= '1';
opcode_r <= TAKE_NEXT_SAMPLE;
if (ack_r = '1') then
stage_next <= WAIT_FOR_READER;
end if;
when WAIT_FOR_READER =>
if (done_r = '1') then
case (return_code_r) 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 (sample_info_valid_r = '1') then
sample_info_ack_r <= '1';
-- Meta Sample
if (sample_info_r.valid_data = '0') then
-- Ignore and read Next Sample
stage_next <= INITIATE_READ;
else
get_data_r <= '1';
stage_next <= GET_PAYLOAD_HEADER;
service_info_sig_next.received_timestamp <= TIME_INVALID;
service_info_sig_next.source_timestamp <= sample_info_r.source_timestamp;
end if;
end if;
when GET_PAYLOAD_HEADER =>
-- TODO: Latch Offset from Options Field?
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
case (representation_id) is
when CDR_BE =>
endian_flag_next <= '0';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
-- Initial Fetch
when CDR_LE =>
endian_flag_next <= '1';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
when others =>
-- Unknown Payload Encoding
stage_next <= SKIP_PAYLOAD;
decode_error_latch_next <= '1';
end case;
end if;
when FETCH =>
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
data_in_latch_next <= data_in_r;
-- Alignment Operation in progress
if (align_op = '1') then
stage_next <= ALIGN_IN_STREAM;
-- Reset
align_op_next <= '0';
else
stage_next <= DECODE_PAYLOAD;
end if;
end if;
when ALIGN_IN_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
-- NOTE: The Cyclone DDS implementation uses a custom request header that is pre-pended to the actual service request/response.
-- It is defined as follows:
-- struct cdds_request_header_t{
-- uint64_t guid;
-- int64_t seq;
-- };
-- 'seq' is set by a counter that is incremented on each "send_request".
-- 'guid' is set to the publication handle of the request writer of the service client.
-- Note that the publication handle is useless for the server, since it is only meanigful localy (i.e. only the client can do something with it)
-- Nevertheless the same 'guid' has to be returned to the client.
when GET_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.writer_guid(0) <= get_sub_vector(endian_swap(endian_flag, dw_latch),0,WORD_WIDTH,TRUE);
service_info_sig_next.request_id.writer_guid(1) <= get_sub_vector(endian_swap(endian_flag, dw_latch),1,WORD_WIDTH,TRUE);
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RID_SN;
cnt_next <= 0;
when others =>
null;
end case;
end if;
when GET_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.sequence_number <= to_double_word(unsigned(endian_swap(endian_flag, dw_latch)));
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RQ_GOAL_ID;
when others =>
null;
end case;
end if;
when GET_RQ_GOAL_ID =>
-- Special Decoding for effieciency (Prevent having to define memory for UUID)
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_IN_STREAM;
else
goal_id_latch_next <= write_sub_vector(goal_id_latch, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE), uuid_cnt, TRUE);
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 (uuid_cnt = G_RQ_GOAL_ID_MAX_DEPTH-1) then
-- ###GENERATED START###
decode_stage_next <= TODO;
-- ###GENERATED END###
uuid_cnt_next <= 0; -- Post-reset
else
uuid_cnt_next <= uuid_cnt + 1;
end if;
end if;
-- ###GENERATED START###
when TODO =>
-- ###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_IN_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_r) = (data_in_r'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') then
-- Skip Read
ready_in_r_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;
when INITIATE_WRITE =>
start_w <= '1';
opcode_w <= WRITE;
if (ack_w = '1') then
stage_next <= WRITE_PAYLOAD_HEADER;
end if;
when WRITE_PAYLOAD_HEADER =>
valid_out_w <= '1';
if (LITTLE_ENDIAN = '0') then
data_out_w <= CDR_BE & x"0000";
else
data_out_w <= CDR_LE & x"0000";
end if;
-- Output Guard
if (ready_out_w = '1') then
stage_next <= ENCODE_PAYLOAD;
-- Reset
align_offset_next <= (others => '0');
data_out_latch_next <= (others => '0');
encode_stage_next <= WRITE_RID_WGUID;
cnt_next <= 0;
end if;
when PUSH =>
-- Mark Last Word
if (finalize_payload = '1') then
last_word_out_w <= '1';
end if;
valid_out_w <= '1';
data_out_w <= data_out_latch;
-- Output Guard
if (ready_out_w = '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_OUT_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_OUT_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
when WRITE_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, get_sub_vector(std_logic_vector(to_unsigned(request_id_user.writer_guid)), 0, 64, TRUE)), 0, WORD_WIDTH, TRUE);
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, get_sub_vector(std_logic_vector(to_unsigned(request_id_user.writer_guid)), 0, 64, TRUE)), 1, WORD_WIDTH, TRUE);
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RID_SN;
cnt_next <= 0;
when others =>
end case;
end if;
when WRITE_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(to_unsigned(request_id_user.sequence_number))), 0, WORD_WIDTH, TRUE);
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(to_unsigned(request_id_user.sequence_number))), 1, WORD_WIDTH, TRUE);
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RR_ACCEPTED;
when others =>
end case;
end if;
when WRITE_RR_ACCEPTED =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_OUT_STREAM;
else
data_out_latch_next <= write_sub_vector(data_out_latch, ("0000000" & accepted), to_integer(align_offset(1 downto 0)), TRUE);
align_offset_next <= align_offset + 1;
encode_stage_next <= WRITE_RR_STAMP;
cnt_next <= 0;
-- Need to fetch next Word
if(align_offset(1 downto 0) = "11") then
stage_next <= PUSH;
end if;
end if;
when WRITE_RR_STAMP =>
-- We intepret the TIME as a double word for efficiency
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
-- WRITE 1/2
when 0 =>
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, get_sub_vector(stamp, 0, WORD_WIDTH, TRUE));
stage_next <= PUSH;
cnt_next <= cnt + 1;
-- WRITE 2/2
when 1 =>
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, get_sub_vector(stamp, 1, WORD_WIDTH, TRUE));
align_offset_next <= align_offset + 8;
-- DONE
stage_next <= PUSH;
finalize_payload_next <= '1';
when others =>
end case;
end if;
when others =>
null;
end case;
when WAIT_FOR_WRITER =>
if (done_w = '1') then
case (return_code_w) 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;
when others =>
null;
end case;
-- OVERREAD GUARD
-- Attempted read on empty input
if (last_word_in_latch = '1' and last_word_in_r = '0' and ready_in_r_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;
encode_stage <= WRITE_RID_WGUID;
decode_stage <= GET_RID_WGUID;
return_stage <= GET_RID_WGUID;
target_align <= ALIGN_1;
cnt <= 0;
endian_flag <= '0';
last_word_in_latch <= '0';
decode_error_latch <= '0';
optional <= '0';
taken_sig <= '0';
align_op <= '0';
finalize_payload <= '0';
data_available_sig <= '0';
align_offset <= (others => '0');
data_in_latch <= (others => '0');
data_out_latch <= (others => '0');
dw_latch <= (others => '0');
return_code_latch <= ROS_RET_OK;
service_info_sig <= EMPTY_SERVICE_INFO;
uuid_cnt <= 0;
goal_id_latch <= (others => '0');
-- ###GENERATED START###
-- RESET SYNC SIGNAL VALUE
-- ###GENERATED END###
else
stage <= stage_next;
encode_stage <= encode_stage_next;
decode_stage <= decode_stage_next;
return_stage <= return_stage_next;
target_align <= target_align_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;
finalize_payload <= finalize_payload_next;
data_available_sig <= data_available_sig_next;
align_offset <= align_offset_next;
data_in_latch <= data_in_latch_next;
data_out_latch <= data_out_latch_next;
dw_latch <= dw_latch_next;
return_code_latch <= return_code_latch_next;
service_info_sig <= service_info_sig_next;
uuid_cnt <= uuid_cnt_next;
goal_id_latch <= goal_id_latch_next;
-- ###GENERATED START###
-- SYNC SIGNALS
-- ###GENERATED END###
end if;
end if;
end process;
end architecture;

706
src/ros2/TEMPLATE_ros_action_result_srv_client.vhd Normal file
View File

@ -0,0 +1,706 @@
-- 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;
entity Fibonacci_ros_action_result_srv_client is
generic (
LITTLE_ENDIAN : std_logic := '0'
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
-- FROM DDS READER
start_r : out std_logic;
ack_r : in std_logic;
opcode_r : out DDS_READER_OPCODE_TYPE;
instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
instance_handle_r : out INSTANCE_HANDLE_TYPE;
max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
get_data_r : out std_logic;
done_r : in std_logic;
return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
valid_in_r : in std_logic;
ready_in_r : out std_logic;
data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_r : in std_logic;
sample_info_r : in SAMPLE_INFO_TYPE;
sample_info_valid_r : in std_logic;
sample_info_ack_r : out std_logic;
eoc_r : in std_logic;
status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- FROM DDS WRITER
start_w : out std_logic;
ack_w : in std_logic;
opcode_w : out DDS_WRITER_OPCODE_TYPE;
instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
source_ts_w : out TIME_TYPE;
max_wait_w : out DURATION_TYPE;
done_w : in std_logic;
return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
valid_out_w : out std_logic;
ready_out_w : in std_logic;
data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_out_w : out std_logic;
valid_in_w : in std_logic;
ready_in_w : out std_logic;
data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_w : in std_logic;
status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- TO USER
start_user : in std_logic;
ack_user : out std_logic;
opcode_user : in ROS_SERVICE_OPCODE_TYPE;
service_info_user : out SERVICE_INFO_TYPE;
sequence_id_user : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
data_available_user : out std_logic;
taken_user : out std_logic;
-- REQUEST
goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0);
-- RESPONSE
status : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- ###GENERATED START###
-- RESULT PORTS
-- ###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_ros_action_result_srv_client is
--*****TYPE DECLARATION*****
-- FSM states. Explained below in detail
type STAGE_TYPE is (IDLE,RETURN_ROS,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_WRITER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_IN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_OUT_STREAM,ENCODE_PAYLOAD);
type ENCODE_STAGE_TYPE is (WRITE_RID_WGUID,WRITE_RID_SN,WRITE_RQ_GOAL_ID);
-- ###GENERATED START###
type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RR_STATUS,TODO);
-- TYPE DECLARATIONS
-- ###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 dw_latch, dw_latch_next : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
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 data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
signal abort_mem : std_logic;
signal ready_in_r_sig : std_logic;
signal taken_sig, taken_sig_next : std_logic;
signal service_info_sig, service_info_sig_next : SERVICE_INFO_TYPE;
signal sequence_id_sig, sequence_id_sig_next : unsigned(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
signal finalize_payload, finalize_payload_next : std_logic;
signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE;
signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE;
signal return_stage, return_stage_next : DECODE_STAGE_TYPE;
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal data_available_sig, data_available_sig_next : std_logic;
signal uuid_cnt, uuid_cnt_next : natural range 0 to R_RQ_GOAL_ID_MAX_DEPTH-1;
signal status_latch, status_latch_next : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- ###GENERATED START###
-- SIGNAL DECLARATIONS
-- ###GENERATED END###
--*****ALIAS DECLARATION*****
alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(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_r(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###
-- MEMORY INSTANTIATIONS
-- ###GENERATED END###
instance_state_r <= ANY_INSTANCE_STATE;
view_state_r <= ANY_VIEW_STATE;
sample_state_r <= ANY_SAMPLE_STATE;
instance_handle_r <= HANDLE_NIL;
max_samples_r <= (others => '0');
instance_handle_in_w <= HANDLE_NIL;
source_ts_w <= TIME_INVALID;
max_wait_w <= DURATION_ZERO;
ready_in_w <= '0'; -- DDS Writer Input is unused
taken_user <= taken_sig;
ready_in_r <= ready_in_r_sig;
service_info_user <= service_info_sig;
sequence_id_user <= std_logic_vector(sequence_id_sig);
data_available_user <= data_available_sig;
status <= status_latch;
-- ###GENERATED START###
-- PORT SIGNAL CONNECTIONS
-- ###GENERATED END###
main_prc : process (all)
variable tmp_length : unsigned(WORD_WIDTH-1 downto 0);
begin
-- DEFAULT
stage_next <= stage;
encode_stage_next <= encode_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;
data_out_latch_next <= data_out_latch;
finalize_payload_next <= finalize_payload;
optional_next <= optional;
taken_sig_next <= taken_sig;
data_in_latch_next <= data_in_latch;
align_op_next <= align_op;
return_code_latch_next <= return_code_latch;
service_info_sig_next <= service_info_sig;
dw_latch_next <= dw_latch;
sequence_id_sig_next <= sequence_id_sig;
data_available_sig_next <= data_available_sig;
ready_in_r_sig <= '0';
abort_mem <= '0';
ack_user <= '0';
sample_info_ack_r <= '0';
get_data_r <= '0';
start_r <= '0';
opcode_r <= NOP;
start_w <= '0';
opcode_w <= NOP;
valid_out_w <= '0';
last_word_out_w <= '0';
done_user <= '0';
return_code_user <= ROS_RET_OK;
data_out_w <= (others => '0');
uuid_cnt_next <= uuid_cnt;
status_latch_next <= status_latch;
-- ###GENERATED START###
-- DEFAULT SIGNAL ASSIGNMENTS
-- ###GENERATED END###
-- Last Word Latch Setter
if (last_word_in_r = '1') then
last_word_in_latch_next <= '1';
end if;
-- Data Available Setter
if (check_mask(status_r, 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 SEND_REQUEST =>
stage_next <= INITIATE_WRITE;
-- Increment Sequence ID
sequence_id_sig_next <= sequence_id_sig + 1;
when TAKE_RESPONSE =>
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###
-- MEMORY SIGNAL CONNECTIONS
-- ###GENERATED END###
end if;
when RETURN_ROS =>
done_user <= '1';
return_code_user <= return_code_latch;
-- DONE
stage_next <= IDLE;
when INITIATE_READ =>
start_r <= '1';
opcode_r <= TAKE_NEXT_SAMPLE;
if (ack_r = '1') then
stage_next <= WAIT_FOR_READER;
end if;
when WAIT_FOR_READER =>
if (done_r = '1') then
case (return_code_r) 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 (sample_info_valid_r = '1') then
sample_info_ack_r <= '1';
-- Meta Sample
if (sample_info_r.valid_data = '0') then
-- Ignore and read Next Sample
stage_next <= INITIATE_READ;
else
get_data_r <= '1';
stage_next <= GET_PAYLOAD_HEADER;
service_info_sig_next.received_timestamp <= TIME_INVALID;
service_info_sig_next.source_timestamp <= sample_info_r.source_timestamp;
end if;
end if;
when GET_PAYLOAD_HEADER =>
-- TODO: Latch Offset from Options Field?
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
case (representation_id) is
when CDR_BE =>
endian_flag_next <= '0';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
-- Initial Fetch
when CDR_LE =>
endian_flag_next <= '1';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
when others =>
-- Unknown Payload Encoding
stage_next <= SKIP_PAYLOAD;
decode_error_latch_next <= '1';
end case;
end if;
when FETCH =>
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
data_in_latch_next <= data_in_r;
-- Alignment Operation in progress
if (align_op = '1') then
stage_next <= ALIGN_IN_STREAM;
-- Reset
align_op_next <= '0';
else
stage_next <= DECODE_PAYLOAD;
end if;
end if;
when ALIGN_IN_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
-- NOTE: The Cyclone DDS implementation uses a custom request header that is pre-pended to the actual service request/response.
-- It is defined as follows:
-- struct cdds_request_header_t{
-- uint64_t guid;
-- int64_t seq;
-- };
-- 'seq' is set by a counter that is incremented on each "send_request".
-- 'guid' is set to the publication handle of the request writer of the service client.
-- Note that the publication handle is useless for the server, since it is only meanigful localy (i.e. only the client can do something with it)
-- Nevertheless the same 'guid' has to be returned to the client.
when GET_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.writer_guid(0) <= get_sub_vector(endian_swap(endian_flag, dw_latch),0,WORD_WIDTH,TRUE);
service_info_sig_next.request_id.writer_guid(1) <= get_sub_vector(endian_swap(endian_flag, dw_latch),1,WORD_WIDTH,TRUE);
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RID_SN;
cnt_next <= 0;
when others =>
null;
end case;
end if;
when GET_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.sequence_number <= to_double_word(unsigned(endian_swap(endian_flag, dw_latch)));
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RR_STATUS;
when others =>
null;
end case;
end if;
when GET_RR_STATUS =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_IN_STREAM;
else
status_latch_next <= endian_swap(endian_flag, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE));
align_offset_next <= align_offset + 1;
-- ###GENERATED START###
decode_stage_next <= TODO;
-- ###GENERATED END###
-- Need to fetch next Word
if(align_offset(1 downto 0) = "11") then
stage_next <= FETCH;
end if;
end if;
-- ###GENERATED START###
WHEN TODO =>
-- ###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_IN_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_r) = (data_in_r'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') then
-- Skip Read
ready_in_r_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;
when INITIATE_WRITE =>
start_w <= '1';
opcode_w <= WRITE;
if (ack_w = '1') then
stage_next <= WRITE_PAYLOAD_HEADER;
end if;
when WRITE_PAYLOAD_HEADER =>
valid_out_w <= '1';
if (LITTLE_ENDIAN = '0') then
data_out_w <= CDR_BE & x"0000";
else
data_out_w <= CDR_LE & x"0000";
end if;
-- Output Guard
if (ready_out_w = '1') then
stage_next <= ENCODE_PAYLOAD;
-- Reset
align_offset_next <= (others => '0');
data_out_latch_next <= (others => '0');
encode_stage_next <= WRITE_RID_WGUID;
cnt_next <= 0;
end if;
when PUSH =>
-- Mark Last Word
if (finalize_payload = '1') then
last_word_out_w <= '1';
end if;
valid_out_w <= '1';
data_out_w <= data_out_latch;
-- Output Guard
if (ready_out_w = '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_OUT_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_OUT_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
when WRITE_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= (others => '0');
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= (others => '0');
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RID_SN;
cnt_next <= 0;
when others =>
end case;
end if;
when WRITE_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(sequence_id_sig)), 0, WORD_WIDTH, TRUE);
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(sequence_id_sig)), 1, WORD_WIDTH, TRUE);
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RQ_GOAL_ID;
when others =>
end case;
end if;
when WRITE_RQ_GOAL_ID =>
-- Special Encoding for effieciency (Prevent having to define memory for UUID)
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_OUT_STREAM;
else
data_out_latch_next <= write_sub_vector(data_out_latch, get_sub_vector(goal_id,uuid_cnt,CDR_INT8_WIDTH,TRUE), to_integer(align_offset(1 downto 0)), TRUE);
align_offset_next <= align_offset + 1;
if (uuid_cnt = R_RQ_MAX_GOAL_ID_SIZE-1) then
-- DONE
stage_next <= PUSH;
finalize_payload_next <= '1';
uuid_cnt_next <= 0; -- Post-Reset
else
uuid_cnt_next <= uuid_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 =>
null;
end case;
when WAIT_FOR_WRITER =>
if (done_w = '1') then
case (return_code_w) 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;
when others =>
null;
end case;
-- OVERREAD GUARD
-- Attempted read on empty input
if (last_word_in_latch = '1' and last_word_in_r = '0' and ready_in_r_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;
encode_stage <= WRITE_RID_WGUID;
decode_stage <= GET_RID_WGUID;
return_stage <= GET_RID_WGUID;
target_align <= ALIGN_1;
cnt <= 0;
endian_flag <= '0';
last_word_in_latch <= '0';
decode_error_latch <= '0';
optional <= '0';
taken_sig <= '0';
align_op <= '0';
finalize_payload <= '0';
data_available_sig <= '0';
align_offset <= (others => '0');
data_in_latch <= (others => '0');
data_out_latch <= (others => '0');
dw_latch <= (others => '0');
sequence_id_sig <= (others => '0');
return_code_latch <= ROS_RET_OK;
service_info_sig <= EMPTY_SERVICE_INFO;
uuid_cnt <= 0;
status_latch <= (others => '0');
-- ###GENERATED START###
-- RESET SYNC SIGNAL VALUE
-- ###GENERATED END###
else
stage <= stage_next;
encode_stage <= encode_stage_next;
decode_stage <= decode_stage_next;
return_stage <= return_stage_next;
target_align <= target_align_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;
finalize_payload <= finalize_payload_next;
data_available_sig <= data_available_sig_next;
align_offset <= align_offset_next;
data_in_latch <= data_in_latch_next;
data_out_latch <= data_out_latch_next;
dw_latch <= dw_latch_next;
sequence_id_sig <= sequence_id_sig_next;
return_code_latch <= return_code_latch_next;
service_info_sig <= service_info_sig_next;
uuid_cnt <= uuid_cnt_next;
status_latch <= status_latch_next;
-- ###GENERATED START###
-- SYNC SIGNALS
-- ###GENERATED END###
end if;
end if;
end process;
end architecture;

699
src/ros2/TEMPLATE_ros_action_result_srv_server.vhd Normal file
View File

@ -0,0 +1,699 @@
-- 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;
entity Fibonacci_ros_action_result_srv_server is
generic (
LITTLE_ENDIAN : std_logic := '0'
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
-- FROM DDS READER
start_r : out std_logic;
ack_r : in std_logic;
opcode_r : out DDS_READER_OPCODE_TYPE;
instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
instance_handle_r : out INSTANCE_HANDLE_TYPE;
max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
get_data_r : out std_logic;
done_r : in std_logic;
return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
valid_in_r : in std_logic;
ready_in_r : out std_logic;
data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_r : in std_logic;
sample_info_r : in SAMPLE_INFO_TYPE;
sample_info_valid_r : in std_logic;
sample_info_ack_r : out std_logic;
eoc_r : in std_logic;
status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- FROM DDS WRITER
start_w : out std_logic;
ack_w : in std_logic;
opcode_w : out DDS_WRITER_OPCODE_TYPE;
instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
source_ts_w : out TIME_TYPE;
max_wait_w : out DURATION_TYPE;
done_w : in std_logic;
return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
valid_out_w : out std_logic;
ready_out_w : in std_logic;
data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_out_w : out std_logic;
valid_in_w : in std_logic;
ready_in_w : out std_logic;
data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
last_word_in_w : in std_logic;
status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- TO USER
start_user : in std_logic;
ack_user : out std_logic;
opcode_user : in ROS_SERVICE_OPCODE_TYPE;
service_info_user : out SERVICE_INFO_TYPE;
request_id_user : in REQUEST_ID_TYPE;
data_available_user : out std_logic;
taken_user : out std_logic;
-- REQUEST
goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
-- RESPONSE
status : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- ###GENERATED START###
-- RESULT PORTS
-- ###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_ros_action_result_srv_server is
--*****TYPE DECLARATION*****
-- FSM states. Explained below in detail
type STAGE_TYPE is (IDLE,RETURN_ROS,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_WRITER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_IN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_OUT_STREAM,ENCODE_PAYLOAD);
type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RQ_GOAL_ID);
-- ###GENERATED START###
type ENCODE_STAGE_TYPE is (WRITE_RID_WGUID,WRITE_RID_SN,WRITE_RR_STATUS,TODO);
-- TYPE DECLARATIONS
-- ###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 dw_latch, dw_latch_next : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
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 data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
signal abort_mem : std_logic;
signal ready_in_r_sig : std_logic;
signal taken_sig, taken_sig_next : std_logic;
signal service_info_sig, service_info_sig_next : SERVICE_INFO_TYPE;
signal finalize_payload, finalize_payload_next : std_logic;
signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE;
signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE;
signal return_stage, return_stage_next : DECODE_STAGE_TYPE;
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal data_available_sig, data_available_sig_next : std_logic;
signal uuid_cnt, uuid_cnt_next : natural range 0 to R_RQ_GOAL_ID_MAX_DEPTH-1;
signal goal_id_latch, goal_id_latch_next : std_logic_vector(UUID_WIDTH-1 downto 0);
-- ###GENERATED START###
-- SIGNAL DECLARATIONS
-- ###GENERATED END###
--*****ALIAS DECLARATION*****
alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(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_r(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###
-- MEMORY INSTANTIATIONS
-- ###GENERATED END###
instance_state_r <= ANY_INSTANCE_STATE;
view_state_r <= ANY_VIEW_STATE;
sample_state_r <= ANY_SAMPLE_STATE;
instance_handle_r <= HANDLE_NIL;
max_samples_r <= (others => '0');
instance_handle_in_w <= HANDLE_NIL;
source_ts_w <= TIME_INVALID;
max_wait_w <= DURATION_ZERO;
ready_in_w <= '0'; -- DDS Writer Input is unused
taken_user <= taken_sig;
ready_in_r <= ready_in_r_sig;
service_info_user <= service_info_sig;
data_available_user <= data_available_sig;
goal_id <= goal_id_latch;
-- ###GENERATED START###
-- PORT SIGNAL CONNECTIONS
-- ###GENERATED END###
main_prc : process (all)
variable tmp_length : unsigned(WORD_WIDTH-1 downto 0);
begin
-- DEFAULT
stage_next <= stage;
encode_stage_next <= encode_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;
data_out_latch_next <= data_out_latch;
finalize_payload_next <= finalize_payload;
optional_next <= optional;
taken_sig_next <= taken_sig;
data_in_latch_next <= data_in_latch;
align_op_next <= align_op;
return_code_latch_next <= return_code_latch;
service_info_sig_next <= service_info_sig;
dw_latch_next <= dw_latch;
data_available_sig_next <= data_available_sig;
ready_in_r_sig <= '0';
abort_mem <= '0';
ack_user <= '0';
sample_info_ack_r <= '0';
get_data_r <= '0';
start_r <= '0';
opcode_r <= NOP;
start_w <= '0';
opcode_w <= NOP;
valid_out_w <= '0';
last_word_out_w <= '0';
done_user <= '0';
return_code_user <= ROS_RET_OK;
data_out_w <= (others => '0');
uuid_cnt_next <= uuid_cnt;
goal_id_latch_next <= goal_id_latch;
-- ###GENERATED START###
-- DEFAULT SIGNAL ASSIGNMENTS
-- ###GENERATED END###
-- Last Word Latch Setter
if (last_word_in_r = '1') then
last_word_in_latch_next <= '1';
end if;
-- Data Available Setter
if (check_mask(status_r, 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_REQUEST =>
stage_next <= INITIATE_READ;
when SEND_RESPONSE =>
stage_next <= INITIATE_WRITE;
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###
-- MEMORY SIGNAL CONNECTIONS
-- ###GENERATED END###
end if;
when RETURN_ROS =>
done_user <= '1';
return_code_user <= return_code_latch;
-- DONE
stage_next <= IDLE;
when INITIATE_READ =>
start_r <= '1';
opcode_r <= TAKE_NEXT_SAMPLE;
if (ack_r = '1') then
stage_next <= WAIT_FOR_READER;
end if;
when WAIT_FOR_READER =>
if (done_r = '1') then
case (return_code_r) 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 (sample_info_valid_r = '1') then
sample_info_ack_r <= '1';
-- Meta Sample
if (sample_info_r.valid_data = '0') then
-- Ignore and read Next Sample
stage_next <= INITIATE_READ;
else
get_data_r <= '1';
stage_next <= GET_PAYLOAD_HEADER;
service_info_sig_next.received_timestamp <= TIME_INVALID;
service_info_sig_next.source_timestamp <= sample_info_r.source_timestamp;
end if;
end if;
when GET_PAYLOAD_HEADER =>
-- TODO: Latch Offset from Options Field?
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
case (representation_id) is
when CDR_BE =>
endian_flag_next <= '0';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
-- Initial Fetch
when CDR_LE =>
endian_flag_next <= '1';
stage_next <= FETCH;
-- Alignment Reset
align_offset_next <= (others => '0');
decode_stage_next <= GET_RID_WGUID;
cnt_next <= 0;
when others =>
-- Unknown Payload Encoding
stage_next <= SKIP_PAYLOAD;
decode_error_latch_next <= '1';
end case;
end if;
when FETCH =>
ready_in_r_sig <= '1';
-- Input Guard
if (valid_in_r = '1') then
data_in_latch_next <= data_in_r;
-- Alignment Operation in progress
if (align_op = '1') then
stage_next <= ALIGN_IN_STREAM;
-- Reset
align_op_next <= '0';
else
stage_next <= DECODE_PAYLOAD;
end if;
end if;
when ALIGN_IN_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
-- NOTE: The Cyclone DDS implementation uses a custom request header that is pre-pended to the actual service request/response.
-- It is defined as follows:
-- struct cdds_request_header_t{
-- uint64_t guid;
-- int64_t seq;
-- };
-- 'seq' is set by a counter that is incremented on each "send_request".
-- 'guid' is set to the publication handle of the request writer of the service client.
-- Note that the publication handle is useless for the server, since it is only meanigful localy (i.e. only the client can do something with it)
-- Nevertheless the same 'guid' has to be returned to the client.
when GET_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.writer_guid(0) <= get_sub_vector(endian_swap(endian_flag, dw_latch),0,WORD_WIDTH,TRUE);
service_info_sig_next.request_id.writer_guid(1) <= get_sub_vector(endian_swap(endian_flag, dw_latch),1,WORD_WIDTH,TRUE);
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RID_SN;
cnt_next <= 0;
when others =>
null;
end case;
end if;
when GET_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_IN_STREAM;
else
case (cnt) is
-- Double Word 1/2
when 0 =>
dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch;
stage_next <= FETCH;
cnt_next <= cnt + 1;
-- Double Word 2/2
when 1 =>
dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch;
cnt_next <= cnt + 1;
-- Push Double Word
when 2 =>
service_info_sig_next.request_id.sequence_number <= to_double_word(unsigned(endian_swap(endian_flag, dw_latch)));
stage_next <= FETCH;
align_offset_next <= align_offset + 8;
decode_stage_next <= GET_RQ_GOAL_ID;
when others =>
null;
end case;
end if;
when GET_RQ_GOAL_ID =>
-- Special Decoding for effieciency (Prevent having to define memory for UUID)
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_IN_STREAM;
else
goal_id_latch_next <= write_sub_vector(goal_id_latch, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE), uuid_cnt, TRUE);
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 (uuid_cnt = R_RQ_GOAL_ID_MAX_DEPTH-1) then
-- DONE
stage_next <= SKIP_PAYLOAD;
uuid_cnt_next <= 0; -- Post-reset
else
uuid_cnt_next <= uuid_cnt + 1;
end if;
end if;
when GET_OPTIONAL_HEADER =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_4)) then
target_align_next <= ALIGN_4;
stage_next <= ALIGN_IN_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_r) = (data_in_r'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') then
-- Skip Read
ready_in_r_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;
when INITIATE_WRITE =>
start_w <= '1';
opcode_w <= WRITE;
if (ack_w = '1') then
stage_next <= WRITE_PAYLOAD_HEADER;
end if;
when WRITE_PAYLOAD_HEADER =>
valid_out_w <= '1';
if (LITTLE_ENDIAN = '0') then
data_out_w <= CDR_BE & x"0000";
else
data_out_w <= CDR_LE & x"0000";
end if;
-- Output Guard
if (ready_out_w = '1') then
stage_next <= ENCODE_PAYLOAD;
-- Reset
align_offset_next <= (others => '0');
data_out_latch_next <= (others => '0');
encode_stage_next <= WRITE_RID_WGUID;
cnt_next <= 0;
end if;
when PUSH =>
-- Mark Last Word
if (finalize_payload = '1') then
last_word_out_w <= '1';
end if;
valid_out_w <= '1';
data_out_w <= data_out_latch;
-- Output Guard
if (ready_out_w = '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_OUT_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_OUT_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
when WRITE_RID_WGUID =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, get_sub_vector(std_logic_vector(to_unsigned(request_id_user.writer_guid)), 0, 64, TRUE)), 0, WORD_WIDTH, TRUE);
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, get_sub_vector(std_logic_vector(to_unsigned(request_id_user.writer_guid)), 0, 64, TRUE)), 1, WORD_WIDTH, TRUE);
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RID_SN;
cnt_next <= 0;
when others =>
end case;
end if;
when WRITE_RID_SN =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_8)) then
target_align_next <= ALIGN_8;
stage_next <= ALIGN_OUT_STREAM;
else
case (cnt) is
when 0 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(to_unsigned(request_id_user.sequence_number))), 0, WORD_WIDTH, TRUE);
stage_next <= PUSH;
cnt_next <= cnt + 1;
when 1 =>
data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(to_unsigned(request_id_user.sequence_number))), 1, WORD_WIDTH, TRUE);
stage_next <= PUSH;
align_offset_next <= align_offset + 8;
encode_stage_next <= WRITE_RR_STATUS;
when others =>
end case;
end if;
when WRITE_RR_STATUS =>
-- ALIGN GUARD
if (not check_align(align_offset, ALIGN_1)) then
target_align_next <= ALIGN_1;
stage_next <= ALIGN_OUT_STREAM;
else
data_out_latch_next <= write_sub_vector(data_out_latch, endian_swap(LITTLE_ENDIAN, status), to_integer(align_offset(1 downto 0)), TRUE);
align_offset_next <= align_offset + 1;
-- ###GENERATED START###
encode_stage_next <= TODO;
-- ###GENERATED END###
-- Need to fetch next Word
if(align_offset(1 downto 0) = "11") then
stage_next <= PUSH;
end if;
end if;
-- ###GENERATED START###
when TODO =>
-- ###GENERATED END###
when others =>
null;
end case;
when WAIT_FOR_WRITER =>
if (done_w = '1') then
case (return_code_w) 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;
when others =>
null;
end case;
-- OVERREAD GUARD
-- Attempted read on empty input
if (last_word_in_latch = '1' and last_word_in_r = '0' and ready_in_r_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;
encode_stage <= WRITE_RID_WGUID;
decode_stage <= GET_RID_WGUID;
return_stage <= GET_RID_WGUID;
target_align <= ALIGN_1;
cnt <= 0;
endian_flag <= '0';
last_word_in_latch <= '0';
decode_error_latch <= '0';
optional <= '0';
taken_sig <= '0';
align_op <= '0';
finalize_payload <= '0';
data_available_sig <= '0';
align_offset <= (others => '0');
data_in_latch <= (others => '0');
data_out_latch <= (others => '0');
dw_latch <= (others => '0');
return_code_latch <= ROS_RET_OK;
service_info_sig <= EMPTY_SERVICE_INFO;
uuid_cnt <= 0;
goal_id_latch <= (others => '0');
-- ###GENERATED START###
-- RESET SYNC SIGNAL VALUE
-- ###GENERATED END###
else
stage <= stage_next;
encode_stage <= encode_stage_next;
decode_stage <= decode_stage_next;
return_stage <= return_stage_next;
target_align <= target_align_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;
finalize_payload <= finalize_payload_next;
data_available_sig <= data_available_sig_next;
align_offset <= align_offset_next;
data_in_latch <= data_in_latch_next;
data_out_latch <= data_out_latch_next;
dw_latch <= dw_latch_next;
return_code_latch <= return_code_latch_next;
service_info_sig <= service_info_sig_next;
uuid_cnt <= uuid_cnt_next;
goal_id_latch <= goal_id_latch_next;
-- ###GENERATED START###
-- SYNC SIGNALS
-- ###GENERATED END###
end if;
end if;
end process;
end architecture;

783
src/ros2/TEMPLATE_ros_action_server.vhd Normal file
View File

@ -0,0 +1,783 @@
-- 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.math_pkg.all;
use work.rtps_package.all;
use work.ros_package.all;
use work.GoalStatus_package;
use work.GoalStatusArray_package;
use work.CancelGoal_package;
entity Fibonacci_ros_action_server is
generic (
TIMEOUT_DURATION : ROS_DURATION_TYPE;
MAX_GOALS : natural;
MAX_RESULT_REQUESTS : natural;
ENABLE_FEEDBACK : std_logic := '1';
LITTLE_ENDIAN : std_logic := '0'
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
time : in ROS_TIME_TYPE;
-- *GOAL SERVICE*
-- DDS READER
g_start_r : out std_logic;
g_ack_r : in std_logic;
g_opcode_r : out DDS_READER_OPCODE_TYPE;
g_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
g_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
g_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
g_instance_handle_r : out INSTANCE_HANDLE_TYPE;
g_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
g_get_data_r : out std_logic;
g_done_r : in std_logic;
g_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
g_valid_in_r : in std_logic;
g_ready_in_r : out std_logic;
g_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
g_last_word_in_r : in std_logic;
g_sample_info_r : in SAMPLE_INFO_TYPE;
g_sample_info_valid_r : in std_logic;
g_sample_info_ack_r : out std_logic;
g_eoc_r : in std_logic;
g_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- DDS WRITER
g_start_w : out std_logic;
g_ack_w : in std_logic;
g_opcode_w : out DDS_WRITER_OPCODE_TYPE;
g_instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
g_source_ts_w : out TIME_TYPE;
g_max_wait_w : out DURATION_TYPE;
g_done_w : in std_logic;
g_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
g_instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
g_valid_out_w : out std_logic;
g_ready_out_w : in std_logic;
g_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
g_last_word_out_w : out std_logic;
g_valid_in_w : in std_logic;
g_ready_in_w : out std_logic;
g_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
g_last_word_in_w : in std_logic;
g_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *RESULT SERVICE*
-- DDS READER
r_start_r : out std_logic;
r_ack_r : in std_logic;
r_opcode_r : out DDS_READER_OPCODE_TYPE;
r_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
r_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
r_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
r_instance_handle_r : out INSTANCE_HANDLE_TYPE;
r_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
r_get_data_r : out std_logic;
r_done_r : in std_logic;
r_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
r_valid_in_r : in std_logic;
r_ready_in_r : out std_logic;
r_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
r_last_word_in_r : in std_logic;
r_sample_info_r : in SAMPLE_INFO_TYPE;
r_sample_info_valid_r : in std_logic;
r_sample_info_ack_r : out std_logic;
r_eoc_r : in std_logic;
r_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- DDS WRITER
r_start_w : out std_logic;
r_ack_w : in std_logic;
r_opcode_w : out DDS_WRITER_OPCODE_TYPE;
r_instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
r_source_ts_w : out TIME_TYPE;
r_max_wait_w : out DURATION_TYPE;
r_done_w : in std_logic;
r_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
r_instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
r_valid_out_w : out std_logic;
r_ready_out_w : in std_logic;
r_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
r_last_word_out_w : out std_logic;
r_valid_in_w : in std_logic;
r_ready_in_w : out std_logic;
r_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
r_last_word_in_w : in std_logic;
r_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *CANCEL SERVICE*
-- DDS READER
c_start_r : out std_logic;
c_ack_r : in std_logic;
c_opcode_r : out DDS_READER_OPCODE_TYPE;
c_instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
c_view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
c_sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
c_instance_handle_r : out INSTANCE_HANDLE_TYPE;
c_max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
c_get_data_r : out std_logic;
c_done_r : in std_logic;
c_return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
c_valid_in_r : in std_logic;
c_ready_in_r : out std_logic;
c_data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0);
c_last_word_in_r : in std_logic;
c_sample_info_r : in SAMPLE_INFO_TYPE;
c_sample_info_valid_r : in std_logic;
c_sample_info_ack_r : out std_logic;
c_eoc_r : in std_logic;
c_status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- DDS WRITER
c_start_w : out std_logic;
c_ack_w : in std_logic;
c_opcode_w : out DDS_WRITER_OPCODE_TYPE;
c_instance_handle_in_w : out INSTANCE_HANDLE_TYPE;
c_source_ts_w : out TIME_TYPE;
c_max_wait_w : out DURATION_TYPE;
c_done_w : in std_logic;
c_return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
c_instance_handle_out_w : in INSTANCE_HANDLE_TYPE;
c_valid_out_w : out std_logic;
c_ready_out_w : in std_logic;
c_data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0);
c_last_word_out_w : out std_logic;
c_valid_in_w : in std_logic;
c_ready_in_w : out std_logic;
c_data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0);
c_last_word_in_w : in std_logic;
c_status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *FEEDBACK PUBLISHER*
f_start_dds : out std_logic;
f_ack_dds : in std_logic;
f_opcode_dds : out DDS_WRITER_OPCODE_TYPE;
f_instance_handle_in_dds : out INSTANCE_HANDLE_TYPE;
f_source_ts_dds : out TIME_TYPE;
f_max_wait_dds : out DURATION_TYPE;
f_done_dds : in std_logic;
f_return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
f_instance_handle_out_dds : in INSTANCE_HANDLE_TYPE;
f_valid_out_dds : out std_logic;
f_ready_out_dds : in std_logic;
f_data_out_dds : out std_logic_vector(WORD_WIDTH-1 downto 0);
f_last_word_out_dds : out std_logic;
f_valid_in_dds : in std_logic;
f_ready_in_dds : out std_logic;
f_data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0);
f_last_word_in_dds : in std_logic;
f_status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *STATUS PUBLISHER*
s_start_dds : out std_logic;
s_ack_dds : in std_logic;
s_opcode_dds : out DDS_WRITER_OPCODE_TYPE;
s_instance_handle_in_dds : out INSTANCE_HANDLE_TYPE;
s_source_ts_dds : out TIME_TYPE;
s_max_wait_dds : out DURATION_TYPE;
s_done_dds : in std_logic;
s_return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
s_instance_handle_out_dds : in INSTANCE_HANDLE_TYPE;
s_valid_out_dds : out std_logic;
s_ready_out_dds : in std_logic;
s_data_out_dds : out std_logic_vector(WORD_WIDTH-1 downto 0);
s_last_word_out_dds : out std_logic;
s_valid_in_dds : in std_logic;
s_ready_in_dds : out std_logic;
s_data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0);
s_last_word_in_dds : in std_logic;
s_status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
-- *USER*
start : in std_logic;
opcode : in ROS_ACTION_OPCODE_TYPE;
ack : out std_logic;
done : out std_logic;
return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
goal_handle_in : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
goal_handle_out : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
goal_state_in : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
goal_state_out : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0);
goal_stamp : out ROS_TIME_TYPE;
-- GOAL
new_goal_request : out std_logic;
new_goal_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
new_goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0);
-- ###GENERATED START###
-- GOAL PORTS (Prepend with new_goal_)
-- ###GENERATED END###
new_goal_accepted : in std_logic;
new_goal_response : in std_logic;
-- CANCEL
cancel_request : out std_logic;
cancel_request_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
cancel_accepted : in std_logic;
cancel_response : in std_logic;
-- RESULT SIGNALS
result_addr : in std_logic_vector(WORD_WIDTH-1 downto 0);
result_ready : out std_logic;
result_ren : in std_logic;
result_wen : in std_logic;
result_valid : out std_logic;
result_ack : in std_logic;
-- ###GENERATED START###
-- RESULT PORTS (Prepend with result_)
-- ###GENERATED END###
-- FEEDBACK SIGNALS
-- ###GENERATED START###
-- FEEDBACK PORTS (Prepend with feedback_)
-- ###GENERATED END###
);
end entity;
architecture arch of Fibonacci_ros_action_server is
--*****CONSTANT DECLARATION*****
constant RESULT_INDEX_WIDTH : natural := log2c(MAX_GOALS);
--*****TYPE DECLARATION*****
type STAGE_TYPE is (IDLE,FETCH_RESULT,PASSTHROUGH,TODO);
-- ###GENERATED START###
-- TYPE DECLARATIONS
-- ###GENERATED END###
--*****SIGNAL DECLARATION*****
signal abort_mem : std_logic;
signal stage, stage_next : STAGE_TYPE;
signal cnt, cnt_next : natural range 0 to 5;
signal g_start_user, r_start_user, c_start_user, g_ack_user, r_ack_user, c_ack_user, g_data_available_user, r_data_available_user, c_data_available_user, g_taken_user, r_taken_user, c_taken_user, g_done_user, r_done_user, c_done_user : std_logic;
signal g_opcode_user, r_opcode_user, c_opcode_user : ROS_SERVICE_OPCODE_TYPE;
signal g_service_info_user, r_service_info_user, c_service_info_user : SERVICE_INFO_TYPE;
signal g_request_id_user, r_request_id_user, c_request_id_user : REQUEST_ID_TYPE;
signal g_return_code_user, r_return_code_user, c_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal g_goal_id, r_goal_id, f_goal_id : std_logic_vector(UUID_WIDTH-1 downto 0);
signal g_accepted : std_logic;
signal g_stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
signal r_status : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
signal r_index : std_logic_vector(WORD_WIDTH-1 downto 0);
signal r_sel, r_sel_ack :std_logic;
signal c_goal_info_goal_id : std_logic_vector(UUID_WIDTH-1 downto 0);
signal c_goal_info_stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
signal c_return_code : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
signal c_goals_canceling_len : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
signal c_goals_canceling_addr : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
signal c_goals_canceling_ready, c_goals_canceling_ren, c_goals_canceling_wen, c_goals_canceling_valid, c_goals_canceling_ack : std_logic;
signal c_goals_canceling_goal_id_r, c_goals_canceling_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0);
signal c_goals_canceling_stamp_r, c_goals_canceling_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
signal s_start_user, f_start_user, s_ack_user, f_ack_user, s_done_user, f_done_user : std_logic;
signal s_opcode_user, f_opcode_user : ROS_TOPIC_OPCODE_TYPE;
signal s_return_code_user, f_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
signal s_status_list_len, s_status_list_addr : std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0);
signal s_status_list_ready, s_status_list_ren, s_status_list_wen, s_status_list_valid, s_status_list_ack : std_logic;
signal s_status_list_goal_info_goal_id_r, s_status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0);
signal s_status_list_goal_info_stamp_r, s_status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
signal s_status_list_status_r, s_status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- ###GENERATED START###
-- SIGNAL DECLARATIONS
-- ###GENERATED END###
begin
goal_srv_server_inst : entity work.Fibonacci_ros_action_goal_srv_server(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_r => g_start_r,
ack_r => g_ack_r,
opcode_r => g_opcode_r,
instance_state_r => g_instance_state_r,
view_state_r => g_view_state_r,
sample_state_r => g_sample_state_r,
instance_handle_r => g_instance_handle_r,
max_samples_r => g_max_samples_r,
get_data_r => g_get_data_r,
done_r => g_done_r,
return_code_r => g_return_code_r,
valid_in_r => g_valid_in_r,
ready_in_r => g_ready_in_r,
data_in_r => g_data_in_r,
last_word_in_r => g_last_word_in_r,
sample_info_r => g_sample_info_r,
sample_info_valid_r => g_sample_info_valid_r,
sample_info_ack_r => g_sample_info_ack_r,
eoc_r => g_eoc_r,
status_r => g_status_r,
start_w => g_start_w,
ack_w => g_ack_w,
opcode_w => g_opcode_w,
instance_handle_out_w => g_instance_handle_out_w,
source_ts_w => g_source_ts_w,
max_wait_w => g_max_wait_w,
done_w => g_done_w,
return_code_w => g_return_code_w,
instance_handle_in_w => g_instance_handle_in_w,
valid_out_w => g_valid_out_w,
ready_out_w => g_ready_out_w,
data_out_w => g_data_out_w,
last_word_out_w => g_last_word_out_w,
valid_in_w => g_valid_in_w,
ready_in_w => g_ready_in_w,
data_in_w => g_data_in_w,
last_word_in_w => g_last_word_in_w,
status_w => g_status_w,
start_user => g_start_user,
ack_user => g_ack_user,
opcode_user => g_opcode_user,
service_info_user => g_service_info_user,
request_id_user => g_request_id_user,
data_available_user => g_data_available_user,
taken_user => g_taken_user,
goal_id => g_goal_id,
-- ###GENERATED START###
-- GOAL PORT CONNECTIONS
-- ###GENERATED END###
accepted => g_accepted,
stamp => g_stamp,
done_user => g_done_user,
return_code_user => g_return_code_user
);
result_srv_server_inst : entity work.Fibonacci_ros_action_result_srv_server(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_r => r_start_r,
ack_r => r_ack_r,
opcode_r => r_opcode_r,
instance_state_r => r_instance_state_r,
view_state_r => r_view_state_r,
sample_state_r => r_sample_state_r,
instance_handle_r => r_instance_handle_r,
max_samples_r => r_max_samples_r,
get_data_r => r_get_data_r,
done_r => r_done_r,
return_code_r => r_return_code_r,
valid_in_r => r_valid_in_r,
ready_in_r => r_ready_in_r,
data_in_r => r_data_in_r,
last_word_in_r => r_last_word_in_r,
sample_info_r => r_sample_info_r,
sample_info_valid_r => r_sample_info_valid_r,
sample_info_ack_r => r_sample_info_ack_r,
eoc_r => r_eoc_r,
status_r => r_status_r,
start_w => r_start_w,
ack_w => r_ack_w,
opcode_w => r_opcode_w,
instance_handle_out_w => r_instance_handle_out_w,
source_ts_w => r_source_ts_w,
max_wait_w => r_max_wait_w,
done_w => r_done_w,
return_code_w => r_return_code_w,
instance_handle_in_w => r_instance_handle_in_w,
valid_out_w => r_valid_out_w,
ready_out_w => r_ready_out_w,
data_out_w => r_data_out_w,
last_word_out_w => r_last_word_out_w,
valid_in_w => r_valid_in_w,
ready_in_w => r_ready_in_w,
data_in_w => r_data_in_w,
last_word_in_w => r_last_word_in_w,
status_w => r_status_w,
start_user => r_start_user,
ack_user => r_ack_user,
opcode_user => r_opcode_user,
service_info_user => r_service_info_user,
request_id_user => r_request_id_user,
data_available_user => r_data_available_user,
taken_user => r_taken_user,
goal_id => r_goal_id,
status => r_status,
-- ###GENERATED START###
-- RESULT PORT CONNECTIONS
-- ###GENERATED END###
done_user => r_done_user,
return_code_user => r_return_code_user
);
cancel_srv_server_inst : entity work.CancelGoal_ros_srv_server(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_r => c_start_r,
ack_r => c_ack_r,
opcode_r => c_opcode_r,
instance_state_r => c_instance_state_r,
view_state_r => c_view_state_r,
sample_state_r => c_sample_state_r,
instance_handle_r => c_instance_handle_r,
max_samples_r => c_max_samples_r,
get_data_r => c_get_data_r,
done_r => c_done_r,
return_code_r => c_return_code_r,
valid_in_r => c_valid_in_r,
ready_in_r => c_ready_in_r,
data_in_r => c_data_in_r,
last_word_in_r => c_last_word_in_r,
sample_info_r => c_sample_info_r,
sample_info_valid_r => c_sample_info_valid_r,
sample_info_ack_r => c_sample_info_ack_r,
eoc_r => c_eoc_r,
status_r => c_status_r,
start_w => c_start_w,
ack_w => c_ack_w,
opcode_w => c_opcode_w,
instance_handle_out_w => c_instance_handle_out_w,
source_ts_w => c_source_ts_w,
max_wait_w => c_max_wait_w,
done_w => c_done_w,
return_code_w => c_return_code_w,
instance_handle_in_w => c_instance_handle_in_w,
valid_out_w => c_valid_out_w,
ready_out_w => c_ready_out_w,
data_out_w => c_data_out_w,
last_word_out_w => c_last_word_out_w,
valid_in_w => c_valid_in_w,
ready_in_w => c_ready_in_w,
data_in_w => c_data_in_w,
last_word_in_w => c_last_word_in_w,
status_w => c_status_w,
start_user => c_start_user,
ack_user => c_ack_user,
opcode_user => c_opcode_user,
service_info_user => c_service_info_user,
request_id_user => c_request_id_user,
data_available_user => c_data_available_user,
taken_user => c_taken_user,
goal_info_goal_id => c_goal_info_goal_id,
goal_info_stamp => c_goal_info_stamp,
return_code => c_return_code,
goals_canceling_len => c_goals_canceling_len,
goals_canceling_addr => c_goals_canceling_addr,
goals_canceling_ready => c_goals_canceling_ready,
goals_canceling_ren => c_goals_canceling_ren,
goals_canceling_wen => c_goals_canceling_wen,
goals_canceling_valid => c_goals_canceling_valid,
goals_canceling_ack => c_goals_canceling_ack,
goals_canceling_goal_id_r => c_goals_canceling_goal_id_r,
goals_canceling_goal_id_w => c_goals_canceling_goal_id_w,
goals_canceling_stamp_r => c_goals_canceling_stamp_r,
goals_canceling_stamp_w => c_goals_canceling_stamp_w,
done_user => c_done_user,
return_code_user => c_return_code_user
);
feedback_gen : if ENABLE_FEEDBACK = '1' generate
feedback_pub_inst : entity work.Fibonacci_ros_action_feedback_pub(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_dds => f_start_dds,
ack_dds => f_ack_dds,
opcode_dds => f_opcode_dds,
instance_handle_out_dds => f_instance_handle_out_dds,
source_ts_dds => f_source_ts_dds,
max_wait_dds => f_max_wait_dds,
done_dds => f_done_dds,
return_code_dds => f_return_code_dds,
instance_handle_in_dds => f_instance_handle_in_dds,
valid_out_dds => f_valid_out_dds,
ready_out_dds => f_ready_out_dds,
data_out_dds => f_data_out_dds,
last_word_out_dds => f_last_word_out_dds,
valid_in_dds => f_valid_in_dds,
ready_in_dds => f_ready_in_dds,
data_in_dds => f_data_in_dds,
last_word_in_dds => f_last_word_in_dds,
status_dds => f_status_dds,
start_user => f_start_user,
opcode_user => f_opcode_user,
ack_user => f_ack_user,
-- ###GENERATED START###
-- FEEDBACK PORT CONNECTIONS
-- ###GENERATED END###
done_user => f_done_user,
return_code_user => f_return_code_user
);
else generate
f_start_dds <= '0';
f_opcode_dds <= NOP;
f_instance_handle_in_dds <= HANDLE_NIL;
f_source_ts_dds <= TIME_INVALID;
f_max_wait_dds <= DURATION_ZERO;
f_valid_out_dds <= '0';
f_data_out_dds <= (others => '0');
f_last_word_out_dds <= '0';
f_ready_in_dds <= '0';
-- ###GENERATED START###
-- DEFAULT FEEDBACK PORT VALUES
-- ###GENERATED END###
end generate;
status_pub_inst : entity work.GoalStatusArray_ros_pub(arch)
generic map (
LITTLE_ENDIAN => LITTLE_ENDIAN
)
port map (
clk => clk,
reset => reset,
start_dds => s_start_dds,
ack_dds => s_ack_dds,
opcode_dds => s_opcode_dds,
instance_handle_out_dds => s_instance_handle_out_dds,
source_ts_dds => s_source_ts_dds,
max_wait_dds => s_max_wait_dds,
done_dds => s_done_dds,
return_code_dds => s_return_code_dds,
instance_handle_in_dds => s_instance_handle_in_dds,
valid_out_dds => s_valid_out_dds,
ready_out_dds => s_ready_out_dds,
data_out_dds => s_data_out_dds,
last_word_out_dds => s_last_word_out_dds,
valid_in_dds => s_valid_in_dds,
ready_in_dds => s_ready_in_dds,
data_in_dds => s_data_in_dds,
last_word_in_dds => s_last_word_in_dds,
status_dds => s_status_dds,
start_user => s_start_user,
opcode_user => s_opcode_user,
ack_user => s_ack_user,
status_list_len => s_status_list_len,
status_list_addr => s_status_list_addr,
status_list_ready => s_status_list_ready,
status_list_ren => s_status_list_ren,
status_list_wen => s_status_list_wen,
status_list_valid => s_status_list_valid,
status_list_ack => s_status_list_ack,
status_list_goal_info_goal_id_r => s_status_list_goal_info_goal_id_r,
status_list_goal_info_goal_id_w => s_status_list_goal_info_goal_id_w,
status_list_goal_info_stamp_r => s_status_list_goal_info_stamp_r,
status_list_goal_info_stamp_w => s_status_list_goal_info_stamp_w,
status_list_status_r => s_status_list_status_r,
status_list_status_w => s_status_list_status_w,
done_user => s_done_user,
return_code_user => s_return_code_user
);
action_server_inst : entity work.ros_action_server(arch)
generic map (
TIMEOUT_DURATION => TIMEOUT_DURATION,
MAX_GOALS => MAX_GOALS,
MAX_RESULT_REQUESTS => MAX_RESULT_REQUESTS,
ENABLE_FEEDBACK => ENABLE_FEEDBACK
)
port map (
clk => clk,
reset => reset,
time => time,
-- *GOAL SERVICE*
start_g => g_start_user,
ack_g => g_ack_user,
opcode_g => g_opcode_user,
service_info_g => g_service_info_user,
request_id_g => g_request_id_user,
data_available_g => g_data_available_user,
taken_g => g_taken_user,
goal_id_g => g_goal_id,
accepted_g => g_accepted,
stamp_g => g_stamp,
done_g => g_done_user,
return_code_g => g_return_code_user,
-- *RESULT SERVICE*
start_r => r_start_user,
ack_r => r_ack_user,
opcode_r => r_opcode_user,
service_info_r => r_service_info_user,
request_id_r => r_request_id_user,
data_available_r => r_data_available_user,
taken_r => r_taken_user,
result_index => r_index,
result_sel => r_sel,
result_sel_ack => r_sel_ack,
goal_id_r => r_goal_id,
status_r => r_status,
done_r => r_done_user,
return_code_r => r_return_code_user,
-- *CANCEL SERVICE*
start_c => c_start_user,
ack_c => c_ack_user,
opcode_c => c_opcode_user,
service_info_c => c_service_info_user,
request_id_c => c_request_id_user,
data_available_c => c_data_available_user,
taken_c => c_taken_user,
goal_info_goal_id_c => c_goal_info_goal_id,
goal_info_stamp_c => c_goal_info_stamp,
cancel_return_code_c => c_return_code,
goals_canceling_len_c => c_goals_canceling_len,
goals_canceling_addr_c => c_goals_canceling_addr,
goals_canceling_ready_c => c_goals_canceling_ready,
goals_canceling_ren_c => c_goals_canceling_ren,
goals_canceling_wen_c => c_goals_canceling_wen,
goals_canceling_valid_c => c_goals_canceling_valid,
goals_canceling_ack_c => c_goals_canceling_ack,
goals_canceling_goal_id_r_c => c_goals_canceling_goal_id_r,
goals_canceling_goal_id_w_c => c_goals_canceling_goal_id_w,
goals_canceling_stamp_r_c => c_goals_canceling_stamp_r,
goals_canceling_stamp_w_c => c_goals_canceling_stamp_w,
done_c => c_done_user,
return_code_c => c_return_code_user,
-- *FEEDBACK TOPIC*
start_fb => f_start_user,
opcode_fb => f_opcode_user,
ack_fb => f_ack_user,
goal_id_fb => f_goal_id,
done_fb => f_done_user,
return_code_fb => f_return_code_user,
-- *STATUS TOPIC*
start_s => s_start_user,
opcode_s => s_opcode_user,
ack_s => s_ack_user,
status_list_len_s => s_status_list_len,
status_list_addr_s => s_status_list_addr,
status_list_ready_s => s_status_list_ready,
status_list_ren_s => s_status_list_ren,
status_list_wen_s => s_status_list_wen,
status_list_valid_s => s_status_list_valid,
status_list_ack_s => s_status_list_ack,
status_list_goal_info_goal_id_r_s => s_status_list_goal_info_goal_id_r,
status_list_goal_info_goal_id_w_s => s_status_list_goal_info_goal_id_w,
status_list_goal_info_stamp_r_s => s_status_list_goal_info_stamp_r,
status_list_goal_info_stamp_w_s => s_status_list_goal_info_stamp_w,
status_list_status_r_s => s_status_list_status_r,
status_list_status_w_s => s_status_list_status_w,
done_s => s_done_user,
return_code_s => s_return_code_user,
-- USER
start => start,
opcode => opcode,
ack => ack,
goal_handle_in => goal_handle_in,
goal_handle_out => goal_handle_out,
goal_state_in => goal_state_in,
goal_state_out => goal_state_out,
goal_id => goal_id,
goal_result_index => goal_result_index,
goal_stamp => goal_stamp,
new_goal_request => new_goal_request,
new_goal_handle => new_goal_handle,
new_goal_result_index => new_goal_result_index,
new_goal_accepted => new_goal_accepted,
new_goal_response => new_goal_response,
cancel_request => cancel_request,
cancel_request_handle => cancel_request_handle,
cancel_accepted => cancel_accepted,
cancel_response => cancel_response,
done => done,
return_code => return_code
);
-- ###GENERATED START###
-- MEMORY INSTANTIATIONS
-- ###GENERATED END###
-- ###GENERATED START###
result_valid <= TODO_mem_valid_out;
-- PORT SIGNAL CONNECTIONS
-- ###GENERATED END###
result_mux : process(all)
begin
-- DEFAULT
stage_next <= stage;
cnt_next <= cnt;
-- DEFAULT Unregistered
result_ready <= '0';
r_sel_ack <= '0';
abort_mem <= '0';
-- ###GENERATED START###
-- DEFAULT SIGNAL ASSIGNMENTS
-- ###GENERATED END###
case (stage) is
when IDLE =>
if (r_sel = '1') then
if (unsigned(r_index) = MAX_GOALS) then
stage_next <= PASSTHROUGH;
else
stage_next <= FETCH_RESULT;
cnt_next <= 0;
end if;
-- Reset
abort_mem <= '1';
else
-- ###GENERATED START###
-- MEMORY SIGNAL CONNECTIONS
-- ###GENERATED END###
end if;
when FETCH_RESULT =>
assert (r_sel = '1') severity FAILURE;
case (cnt) is
-- GET
when 0 =>
-- ###GENERATED START###
TODO_mem_addr <= std_logic_vector(resize(unsigned(r_index), RESULT_INDEX_WIDTH));
TODO_mem_read <= '1';
-- Memory Operation Guard
if (TODO_mem_ready_in = '1') then -- AND all 'ready_in' signals
TODO_mem_valid_in <= '1';
cnt_next <= cnt + 1;
end if;
-- ###GENERATED END###
-- READ
when 1 =>
-- ###GENERATED START###
if (TODO_mem_valid_out = '1') then -- AND all 'valid_out' Signals
stage_next <= TODO;
cnt_next <= 0;
end if;
-- ###GENERATED END###
when others =>
null;
end case;
-- ###GENERATED START###
when TODO =>
-- ###GENERATED END###
when PASSTHROUGH =>
r_sel_ack <= '1';
if (unsigned(r_index) = MAX_GOALS) then
-- ###GENERATED START###
-- EMPTY RESULT
-- ###GENERATED END###
else
-- ###GENERATED START###
-- RESULT PASSTHROUGH (data_out memory Signals)
-- ###GENERATED END###
end if;
if (r_sel = '0') then
stage_next <= IDLE;
-- Reset
abort_mem <= '1';
end if;
end case;
end process;
sync_prc : process(clk)
begin
if rising_edge(clk) then
if (reset = '1') then
stage <= IDLE;
cnt <= 0;
else
stage <= stage_next;
cnt <= cnt_next;
end if;
end if;
end process;
end architecture;

19
src/ros2/Tests/Fibonacci.vhd
View File

@ -43,9 +43,12 @@ entity Fibonacci is
cancel_accepted : out std_logic; cancel_accepted : out std_logic;
cancel_response : out std_logic; cancel_response : out std_logic;
-- RESULT SIGNALS -- RESULT SIGNALS
index_result : out std_logic_vector(WORD_WIDTH-1 downto 0); result_addr : out std_logic_vector(WORD_WIDTH-1 downto 0);
index_result_wen : out std_logic; result_ready : in std_logic;
index_result_ready : in std_logic; result_ren : out std_logic;
result_wen : out std_logic;
result_valid : in std_logic;
result_ack : out std_logic;
result_seq_len : out std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); result_seq_len : out std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
result_seq_addr : out std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); result_seq_addr : out std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
result_seq_ready : in std_logic; result_seq_ready : in std_logic;
@ -84,7 +87,9 @@ architecture arch of Fibonacci is
begin begin
index_result <= ind; result_addr <= ind;
result_ren <= '0';
result_ack <= '0';
feedback_seq_len <= std_logic_vector(i); feedback_seq_len <= std_logic_vector(i);
result_seq_len <= std_logic_vector(i); result_seq_len <= std_logic_vector(i);
@ -106,7 +111,7 @@ begin
cancel_accepted <= '0'; cancel_accepted <= '0';
cancel_response <= '0'; cancel_response <= '0';
start <= '0'; start <= '0';
index_result_wen <= '0'; result_wen <= '0';
opcode <= NOP; opcode <= NOP;
goal_handle_in <= GOAL_HANDLE_UNKNOWN; goal_handle_in <= GOAL_HANDLE_UNKNOWN;
goal_state_in <= GoalStatus_package.STATUS_UNKNOWN; goal_state_in <= GoalStatus_package.STATUS_UNKNOWN;
@ -289,9 +294,9 @@ begin
when FINALIZE => when FINALIZE =>
case (cnt) is case (cnt) is
when 0 => when 0 =>
index_result_wen <= '1'; result_wen <= '1';
if (index_result_ready = '1') then if (result_ready = '1') then
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
end if; end if;
when 1 => when 1 =>

22
src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd
View File

@ -74,8 +74,9 @@ architecture testbench of L1_Fibonacci_ros_action_test1 is
signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0'); signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0');
signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0');
signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
signal goal_result_index_s, new_goal_result_index_s, index_result_s : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); signal goal_result_index_s, new_goal_result_index_s, result_addr_s : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
signal new_goal_request_s, new_goal_response_s, index_result_wen_s, index_result_ready_s : std_logic := '0'; signal new_goal_request_s, new_goal_response_s, result_ready_s, result_ren_s, result_wen_s, result_valid_s, result_ack_s : std_logic := '0';
begin begin
uut_c : entity work.Fibonacci_ros_action_client(arch) uut_c : entity work.Fibonacci_ros_action_client(arch)
@ -453,9 +454,12 @@ begin
cancel_request_handle => open, cancel_request_handle => open,
cancel_accepted => '1', cancel_accepted => '1',
cancel_response => '1', cancel_response => '1',
index_result => index_result_s, result_addr => result_addr_s,
index_result_wen => index_result_wen_s, result_ready => result_ready_s,
index_result_ready => index_result_ready_s, result_ren => result_ren_s,
result_wen => result_wen_s,
result_valid => result_valid_s,
result_ack => result_ack_s,
-- ###GENERATED START### -- ###GENERATED START###
result_seq_len => result_seq_len_s, result_seq_len => result_seq_len_s,
result_seq_addr => result_seq_addr_s, result_seq_addr => result_seq_addr_s,
@ -677,7 +681,7 @@ begin
for j in 0 to MAX_GOALS-1 loop for j in 0 to MAX_GOALS-1 loop
Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO);
index_result_s <= int(j,index_result_s'length); result_addr_s <= int(j,result_addr_s'length);
for i in 0 to RV.RandInt(1,10) loop for i in 0 to RV.RandInt(1,10) loop
result_seq_len_s <= int(i+1,result_seq_len_s'length); result_seq_len_s <= int(i+1,result_seq_len_s'length);
result_seq_addr_s <= int(i,result_seq_addr_s'length); result_seq_addr_s <= int(i,result_seq_addr_s'length);
@ -687,10 +691,10 @@ begin
wait until rising_edge(clk); wait until rising_edge(clk);
result_seq_wen_s <= '0'; result_seq_wen_s <= '0';
end loop; end loop;
wait_on_sig(index_result_ready_s); wait_on_sig(result_ready_s);
index_result_wen_s <= '1'; result_wen_s <= '1';
wait until rising_edge(clk); wait until rising_edge(clk);
index_result_wen_s <= '0'; result_wen_s <= '0';
goal_handle_in_s <= test_handle(j); goal_handle_in_s <= test_handle(j);
goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED; goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED;
server_op(UPDATE_GOAL_STATE,ROS_RET_OK); server_op(UPDATE_GOAL_STATE,ROS_RET_OK);

22
src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd
View File

@ -74,8 +74,9 @@ architecture testbench of L1_Fibonacci_ros_action_test2 is
signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0'); signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0');
signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0');
signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
signal goal_result_index_s, new_goal_result_index_s, index_result_s : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); signal goal_result_index_s, new_goal_result_index_s, result_addr_s : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
signal new_goal_request_s, new_goal_response_s, index_result_wen_s, index_result_ready_s : std_logic := '0'; signal new_goal_request_s, new_goal_response_s, result_ready_s, result_ren_s, result_wen_s, result_valid_s, result_ack_s : std_logic := '0';
begin begin
uut_c : entity work.Fibonacci_ros_action_client(arch) uut_c : entity work.Fibonacci_ros_action_client(arch)
@ -457,9 +458,12 @@ begin
cancel_request_handle => open, cancel_request_handle => open,
cancel_accepted => '1', cancel_accepted => '1',
cancel_response => '1', cancel_response => '1',
index_result => index_result_s, result_addr => result_addr_s,
index_result_wen => index_result_wen_s, result_ready => result_ready_s,
index_result_ready => index_result_ready_s, result_ren => result_ren_s,
result_wen => result_wen_s,
result_valid => result_valid_s,
result_ack => result_ack_s,
-- ###GENERATED START### -- ###GENERATED START###
result_seq_len => result_seq_len_s, result_seq_len => result_seq_len_s,
result_seq_addr => result_seq_addr_s, result_seq_addr => result_seq_addr_s,
@ -648,7 +652,7 @@ begin
for j in 0 to MAX_GOALS-1 loop for j in 0 to MAX_GOALS-1 loop
Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO); Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO);
index_result_s <= int(j,index_result_s'length); result_addr_s <= int(j,result_addr_s'length);
for i in 0 to RV.RandInt(1,10) loop for i in 0 to RV.RandInt(1,10) loop
result_seq_len_s <= int(i+1,result_seq_len_s'length); result_seq_len_s <= int(i+1,result_seq_len_s'length);
result_seq_addr_s <= int(i,result_seq_addr_s'length); result_seq_addr_s <= int(i,result_seq_addr_s'length);
@ -658,10 +662,10 @@ begin
wait until rising_edge(clk); wait until rising_edge(clk);
result_seq_wen_s <= '0'; result_seq_wen_s <= '0';
end loop; end loop;
wait_on_sig(index_result_ready_s); wait_on_sig(result_ready_s);
index_result_wen_s <= '1'; result_wen_s <= '1';
wait until rising_edge(clk); wait until rising_edge(clk);
index_result_wen_s <= '0'; result_wen_s <= '0';
goal_handle_in_s <= test_handle(j); goal_handle_in_s <= test_handle(j);
goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED; goal_state_in_s <= GoalStatus_package.STATUS_SUCCEEDED;
server_op(UPDATE_GOAL_STATE,ROS_RET_OK); server_op(UPDATE_GOAL_STATE,ROS_RET_OK);

23
src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd
View File

@ -91,9 +91,9 @@ architecture arch of L2_Testbench_ROS_Lib4 is
signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s, cancel_request_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s, cancel_request_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
signal goal_id_s : std_logic_vector(UUID_WIDTH-1 downto 0); signal goal_id_s : std_logic_vector(UUID_WIDTH-1 downto 0);
signal goal_result_index_s, new_goal_result_index_s, index_result_s : std_logic_vector(WORD_WIDTH-1 downto 0); signal goal_result_index_s, new_goal_result_index_s, result_addr_s : std_logic_vector(WORD_WIDTH-1 downto 0);
signal goal_stamp_s : ROS_TIME_TYPE; signal goal_stamp_s : ROS_TIME_TYPE;
signal new_goal_request_s, new_goal_accepted_s, new_goal_response_s, cancel_request_s, cancel_accepted_s, cancel_response_s, index_result_wen_s, index_result_ready_s : std_logic; signal new_goal_request_s, new_goal_accepted_s, new_goal_response_s, cancel_request_s, cancel_accepted_s, cancel_response_s, result_ready_s, result_ren_s, result_wen_s, result_valid_s, result_ack_s : std_logic;
signal new_goal_order_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); signal new_goal_order_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
signal result_seq_len_s, result_seq_addr_s : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); signal result_seq_len_s, result_seq_addr_s : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
signal result_seq_ready_s, result_seq_ren_s, result_seq_wen_s, result_seq_valid_s, result_seq_ack_s : std_logic; signal result_seq_ready_s, result_seq_ren_s, result_seq_wen_s, result_seq_valid_s, result_seq_ack_s : std_logic;
@ -102,7 +102,6 @@ architecture arch of L2_Testbench_ROS_Lib4 is
signal feedback_seq_ready_s, feedback_seq_ren_s, feedback_seq_wen_s, feedback_seq_valid_s, feedback_seq_ack_s : std_logic; signal feedback_seq_ready_s, feedback_seq_ren_s, feedback_seq_wen_s, feedback_seq_valid_s, feedback_seq_ack_s : std_logic;
signal feedback_seq_r_s, feedback_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); signal feedback_seq_r_s, feedback_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
-- ######GENERATED END###### -- ######GENERATED END######
begin begin
rtps_handler_inst : entity work.rtps_handler(arch) rtps_handler_inst : entity work.rtps_handler(arch)
@ -880,9 +879,12 @@ begin
cancel_request_handle => cancel_request_handle_s, cancel_request_handle => cancel_request_handle_s,
cancel_accepted => cancel_accepted_s, cancel_accepted => cancel_accepted_s,
cancel_response => cancel_response_s, cancel_response => cancel_response_s,
index_result => index_result_s, result_addr => result_addr_s,
index_result_wen => index_result_wen_s, result_ready => result_ready_s,
index_result_ready => index_result_ready_s, result_ren => result_ren_s,
result_wen => result_wen_s,
result_valid => result_valid_s,
result_ack => result_ack_s,
result_seq_len => result_seq_len_s, result_seq_len => result_seq_len_s,
result_seq_addr => result_seq_addr_s, result_seq_addr => result_seq_addr_s,
result_seq_ready => result_seq_ready_s, result_seq_ready => result_seq_ready_s,
@ -931,9 +933,12 @@ begin
cancel_request_handle => cancel_request_handle_s, cancel_request_handle => cancel_request_handle_s,
cancel_accepted => cancel_accepted_s, cancel_accepted => cancel_accepted_s,
cancel_response => cancel_response_s, cancel_response => cancel_response_s,
index_result => index_result_s, result_addr => result_addr_s,
index_result_wen => index_result_wen_s, result_ready => result_ready_s,
index_result_ready => index_result_ready_s, result_ren => result_ren_s,
result_wen => result_wen_s,
result_valid => result_valid_s,
result_ack => result_ack_s,
result_seq_len => result_seq_len_s, result_seq_len => result_seq_len_s,
result_seq_addr => result_seq_addr_s, result_seq_addr => result_seq_addr_s,
result_seq_ready => result_seq_ready_s, result_seq_ready => result_seq_ready_s,

88
src/ros2/example_interfaces/Fibonacci_ros_action_server.vhd
View File

@ -215,9 +215,12 @@ entity Fibonacci_ros_action_server is
cancel_accepted : in std_logic; cancel_accepted : in std_logic;
cancel_response : in std_logic; cancel_response : in std_logic;
-- RESULT SIGNALS -- RESULT SIGNALS
index_result : in std_logic_vector(WORD_WIDTH-1 downto 0); result_addr : in std_logic_vector(WORD_WIDTH-1 downto 0);
index_result_wen : in std_logic; result_ready : out std_logic;
index_result_ready : out std_logic; result_ren : in std_logic;
result_wen : in std_logic;
result_valid : out std_logic;
result_ack : in std_logic;
-- ###GENERATED START### -- ###GENERATED START###
result_seq_len : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); result_seq_len : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
result_seq_addr : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); result_seq_addr : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
@ -250,7 +253,7 @@ architecture arch of Fibonacci_ros_action_server is
constant RESULT_INDEX_WIDTH : natural := log2c(MAX_GOALS); constant RESULT_INDEX_WIDTH : natural := log2c(MAX_GOALS);
--*****TYPE DECLARATION***** --*****TYPE DECLARATION*****
type STAGE_TYPE is (IDLE,GET_RESULT,READ_RESULT,PASSTHROUGH,PUSH_SEQUENCE); type STAGE_TYPE is (IDLE,FETCH_RESULT,PASSTHROUGH,PUSH_SEQUENCE);
-- ###GENERATED START### -- ###GENERATED START###
type R_SEQ_ADDR_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); type R_SEQ_ADDR_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
type R_SEQ_DATA_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(CDR_LONG_WIDTH-1 downto 0); type R_SEQ_DATA_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
@ -287,7 +290,6 @@ architecture arch of Fibonacci_ros_action_server is
signal s_status_list_goal_info_goal_id_r, s_status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0); signal s_status_list_goal_info_goal_id_r, s_status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0);
signal s_status_list_goal_info_stamp_r, s_status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); signal s_status_list_goal_info_stamp_r, s_status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
signal s_status_list_status_r, s_status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); signal s_status_list_status_r, s_status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
signal result_index_latch, result_index_latch_next : std_logic_vector(RESULT_INDEX_WIDTH-1 downto 0);
-- ###GENERATED START### -- ###GENERATED START###
signal seq_len_sig, seq_addr_sig : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0); signal seq_len_sig, seq_addr_sig : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
signal seq_ready_sig, seq_wen_sig : std_logic; signal seq_ready_sig, seq_wen_sig : std_logic;
@ -770,14 +772,21 @@ begin
end generate; end generate;
-- ###GENERATED END### -- ###GENERATED END###
-- ###GENERATED START###
result_valid <= r_seq_len_mem_valid_out;
r_seq_len_mem_data_in <= result_seq_len;
result_seq_valid <= r_seq_mem_valid_out(to_integer(unsigned(result_addr)));
result_seq_r <= r_seq_mem_data_out(to_integer(unsigned(result_addr)));
r_seq_mem_data_in <= (others => result_seq_w);
-- ###GENERATED END###
result_mux : process(all) result_mux : process(all)
begin begin
-- DEFAULT -- DEFAULT
stage_next <= stage; stage_next <= stage;
cnt_next <= cnt; cnt_next <= cnt;
result_index_latch_next <= result_index_latch;
-- DEFAULT Unregistered -- DEFAULT Unregistered
index_result_ready <= '0'; result_ready <= '0';
r_sel_ack <= '0'; r_sel_ack <= '0';
abort_mem <= '0'; abort_mem <= '0';
-- ###GENERATED START### -- ###GENERATED START###
@ -785,15 +794,11 @@ begin
r_seq_len_mem_addr <= (others => '0'); r_seq_len_mem_addr <= (others => '0');
r_seq_len_mem_read <= '0'; r_seq_len_mem_read <= '0';
r_seq_len_mem_valid_in <= '0'; r_seq_len_mem_valid_in <= '0';
r_seq_len_mem_data_in <= (others => '0');
r_seq_len_mem_ready_out <= '0'; r_seq_len_mem_ready_out <= '0';
r_seq_mem_addr <= (others => (others => '0')); r_seq_mem_addr <= (others => (others => '0'));
r_seq_mem_read <= (others => '0'); r_seq_mem_read <= (others => '0');
r_seq_mem_valid_in <= (others => '0'); r_seq_mem_valid_in <= (others => '0');
r_seq_mem_data_in <= (others => (others => '0'));
r_seq_mem_ready_out <= (others => '0'); r_seq_mem_ready_out <= (others => '0');
result_seq_r <= (others => '0');
result_seq_valid <= '0';
result_seq_ready <= '0'; result_seq_ready <= '0';
seq_len_sig <= (others => '0'); seq_len_sig <= (others => '0');
seq_addr_sig <= (others => '0'); seq_addr_sig <= (others => '0');
@ -807,55 +812,54 @@ begin
if (unsigned(r_index) = MAX_GOALS) then if (unsigned(r_index) = MAX_GOALS) then
stage_next <= PASSTHROUGH; stage_next <= PASSTHROUGH;
else else
stage_next <= GET_RESULT; stage_next <= FETCH_RESULT;
cnt_next <= 0;
end if; end if;
-- Reset -- Reset
abort_mem <= '1'; abort_mem <= '1';
else else
index_result_ready <= r_seq_len_mem_ready_in; -- AND all memories
-- ###GENERATED START### -- ###GENERATED START###
r_seq_len_mem_data_in <= result_seq_len; result_ready <= r_seq_len_mem_ready_in;
r_seq_len_mem_addr <= std_logic_vector(resize(unsigned(index_result), RESULT_INDEX_WIDTH));
r_seq_len_mem_valid_in <= index_result_wen;
result_seq_r <= r_seq_mem_data_out(to_integer(unsigned(result_index_latch))); r_seq_len_mem_addr <= std_logic_vector(resize(unsigned(result_addr), RESULT_INDEX_WIDTH));
result_seq_valid <= r_seq_mem_valid_out(to_integer(unsigned(result_index_latch))); r_seq_len_mem_read <= result_ren;
r_seq_mem_data_in <= (others => result_seq_w); r_seq_len_mem_valid_in <= result_ren or result_wen;
result_seq_ready <= r_seq_mem_ready_in(to_integer(unsigned(index_result))); r_seq_len_mem_ready_out <= result_ack;
r_seq_mem_addr(to_integer(unsigned(index_result))) <= result_seq_addr;
r_seq_mem_read(to_integer(unsigned(index_result))) <= result_seq_ren; result_seq_ready <= r_seq_mem_ready_in(to_integer(unsigned(result_addr)));
r_seq_mem_valid_in(to_integer(unsigned(index_result))) <= result_seq_ren or result_seq_wen; r_seq_mem_addr(to_integer(unsigned(result_addr))) <= result_seq_addr;
r_seq_mem_read(to_integer(unsigned(result_addr))) <= result_seq_ren;
r_seq_mem_valid_in(to_integer(unsigned(result_addr))) <= result_seq_ren or result_seq_wen;
r_seq_mem_ready_out <= (others => result_seq_ack); r_seq_mem_ready_out <= (others => result_seq_ack);
-- Latch Address for output
if (result_seq_ren = '1') then
result_index_latch_next <= std_logic_vector(resize(unsigned(index_result), RESULT_INDEX_WIDTH));
end if;
-- ###GENERATED END### -- ###GENERATED END###
end if; end if;
when GET_RESULT => when FETCH_RESULT =>
assert (r_sel = '1') severity FAILURE; assert (r_sel = '1') severity FAILURE;
case (cnt) is
-- GET
when 0 =>
-- ###GENERATED START### -- ###GENERATED START###
r_seq_len_mem_addr <= std_logic_vector(resize(unsigned(r_index), RESULT_INDEX_WIDTH)); r_seq_len_mem_addr <= std_logic_vector(resize(unsigned(r_index), RESULT_INDEX_WIDTH));
r_seq_len_mem_read <= '1'; r_seq_len_mem_read <= '1';
-- Memory Operation Guard -- Memory Operation Guard
if (r_seq_len_mem_ready_in = '1') then -- AND all 'ready_in' signals if (r_seq_len_mem_ready_in = '1') then
-- NOTE: We are toggling the 'valid_in' signal only after we make sure all memories are ready
r_seq_len_mem_valid_in <= '1'; r_seq_len_mem_valid_in <= '1';
stage_next <= READ_RESULT; cnt_next <= cnt + 1;
end if; end if;
-- ###GENERATED END### -- ###GENERATED END###
when READ_RESULT => -- READ
assert (r_sel = '1') severity FAILURE; when 1 =>
-- ###GENERATED START### -- ###GENERATED START###
if (r_seq_len_mem_valid_out = '1') then -- AND all 'valid_out' Signals if (r_seq_len_mem_valid_out = '1') then
stage_next <= PUSH_SEQUENCE; stage_next <= PUSH_SEQUENCE;
cnt_next <= 0; cnt_next <= 0;
seq_cnt_next <= 0; seq_cnt_next <= 0;
end if; end if;
-- ###GENERATED END### -- ###GENERATED END###
when others =>
null;
end case;
-- ###GENERATED START### -- ###GENERATED START###
when PUSH_SEQUENCE => when PUSH_SEQUENCE =>
assert (r_sel = '1') severity FAILURE; assert (r_sel = '1') severity FAILURE;
@ -894,13 +898,15 @@ begin
when PASSTHROUGH => when PASSTHROUGH =>
r_sel_ack <= '1'; r_sel_ack <= '1';
-- ###GENERATED START###
if (unsigned(r_index) = MAX_GOALS) then if (unsigned(r_index) = MAX_GOALS) then
-- ###GENERATED START###
seq_len_sig <= (others => '0'); seq_len_sig <= (others => '0');
else
seq_len_sig <= r_seq_len_mem_data_out;
end if;
-- ###GENERATED END### -- ###GENERATED END###
else
-- ###GENERATED START###
seq_len_sig <= r_seq_len_mem_data_out;
-- ###GENERATED END###
end if;
if (r_sel = '0') then if (r_sel = '0') then
stage_next <= IDLE; stage_next <= IDLE;
@ -917,12 +923,10 @@ begin
stage <= IDLE; stage <= IDLE;
cnt <= 0; cnt <= 0;
seq_cnt <= 0; seq_cnt <= 0;
result_index_latch <= (others => '0');
else else
stage <= stage_next; stage <= stage_next;
cnt <= cnt_next; cnt <= cnt_next;
seq_cnt <= seq_cnt_next; seq_cnt <= seq_cnt_next;
result_index_latch <= result_index_latch_next;
end if; end if;
end if; end if;
end process; end process;