Add RTPS to ROS time converter

src/ros2/ros_time_converter.vhd

@@ -0,0 +1,82 @@
+-- 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.ros_package.all;
+use work.user_config.all;
+
+-- This Entity converts from RTPS/DDS Time (TIME_TYPE) to ROS Time (ROS_TIME_TYPE).
+-- TIME_TYPE is defined as a 64-bit seconds value in 32Q32 fixed point format.
+-- ROS_TIME_TYPE is defined as 32-bit seconds and 32-bit nanoseconds
+-- This entity uses a multiplier to calculate the nanoseconds from the fraction part of the seconds.
+-- Since this entity has an internal pipeline, the time is adjusted by the pipeline delay.
+-- This entity has a minimum delay of 2 clock cycles (1 for the multiplier and 1 for the time adjustment).
+
+entity ros_time_converter is
+    generic (
+        PIPELINE_STAGES : natural := 2
+    );
+    port (
+        -- SYSTEM
+        clk         : in std_logic;
+        reset       : in std_logic;
+        time_in     : in TIME_TYPE;
+        time_out    : out ROS_TIME_TYPE
+    );
+end entity;
+
+architecture arch of ros_time_converter is
+    
+    type SECOND_DELAY_ARRAY is array (0 to PIPELINE_STAGES-2) of std_logic_vector(31 downto 0);
+    
+    -- This is the TIME_TYPE(1) value of 1 nanosecond
+    -- 0.23283064365 in 0Q54 Fixed Point
+    constant NANOSECOND_CONSTANT : std_logic_vector(53 downto 0) := "001110111001101011001001111111111111101110111110110011";
+    
+    signal res : std_logic_vector(85 downto 0);
+    signal time_adjust : TIME_TYPE;
+    signal second_delay : SECOND_DELAY_ARRAY;
+    
+begin
+    
+    assert (PIPELINE_STAGES >= 2) report "PIPELINE_STAGES has to be at least 2" severity FAILURE;
+    
+    mult_inst : configuration work.mult_cfg
+        generic map (
+            PIPELINE_STAGES => PIPELINE_STAGES-1,
+            DATAA_WIDTH     => 32,
+            DATAB_WIDTH     => 54,
+            DATAB_CONST     => TRUE
+        )
+        port map (
+            clk	    => clk,
+            reset   => reset,
+            dataa	=> std_logic_vector(time_adjust(1)),
+            datab	=> NANOSECOND_CONSTANT,
+            result  => res
+        );
+    
+    time_out    <= (sec => second_delay(second_delay'length-1), nanosec => res(85 downto 54));
+    
+    sync_prc : process(clk)
+    begin
+        if rising_edge(clk) then
+            time_adjust <= time_in + gen_duration(CLOCK_PERIOD * PIPELINE_STAGES);
+            if (reset = '1') then
+                second_delay    <= (others => (others => '0'));
+            else
+                second_delay(0) <= std_logic_vector(time_adjust(0));
+                if (second_delay'length >= 1) then
+                    for i in 1 to second_delay'length-1 loop
+                        second_delay(i) <= second_delay(i-1);
+                    end loop;
+                end if;
+            end if;
+        end if;
+    end process;
+    
+end architecture;