Livt packages
Livt.Math
Version 1.7.2
Reusable integer and fixed-point arithmetic, complex numbers, trigonometry, and statistics for FPGA applications written in Livt.
Published files (90)
src/trigonometry/CordicAtan2MagnitudeQ15Primitive.vhd
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.livt_lang_icontext_package.all;
entity livt_math_trigonometry_cordicatan2magnitudeq15primitive is
port (
ctor_lvt_context_in : in t_icontext_in;
ctor_request : in std_logic;
ctor_xbits : in std_logic_vector(15 downto 0);
ctor_ybits : in std_logic_vector(15 downto 0);
ctor_valid : in boolean;
ctor_acknowledge : out std_logic;
ctor_phase : out signed(31 downto 0);
ctor_magnitude : out unsigned(31 downto 0);
ctor_success : out boolean
);
end;
architecture rtl of livt_math_trigonometry_cordicatan2magnitudeq15primitive is
subtype coordinate_t is signed(19 downto 0);
subtype angle_t is signed(20 downto 0);
subtype product_t is signed(38 downto 0);
subtype phase_t is signed(15 downto 0);
subtype magnitude_t is unsigned(15 downto 0);
type worker_state_t is (
idle,
normalize,
vector,
capture_phase,
denormalize,
scale_gain,
finish
);
signal worker_state : worker_state_t := idle;
signal pending : std_logic := '0';
signal x, y : coordinate_t := (others => '0');
signal accumulated_phase : angle_t := (others => '0');
signal normalization : natural range 0 to 14 := 0;
signal normalize_step : natural range 0 to 13 := 0;
signal iteration : natural range 0 to 15 := 0;
signal denormalize_step : natural range 0 to 13 := 0;
signal gain_step : natural range 0 to 6 := 0;
signal gain_accumulator : product_t := (others => '0');
signal phase_result : phase_t := (others => '0');
signal magnitude_result : magnitude_t := (others => '0');
function atan_phase(index : natural) return angle_t is
begin
case index is
when 0 => return to_signed(131072, angle_t'length);
when 1 => return to_signed(77376, angle_t'length);
when 2 => return to_signed(40884, angle_t'length);
when 3 => return to_signed(20753, angle_t'length);
when 4 => return to_signed(10417, angle_t'length);
when 5 => return to_signed(5213, angle_t'length);
when 6 => return to_signed(2607, angle_t'length);
when 7 => return to_signed(1304, angle_t'length);
when 8 => return to_signed(652, angle_t'length);
when 9 => return to_signed(326, angle_t'length);
when 10 => return to_signed(163, angle_t'length);
when 11 => return to_signed(81, angle_t'length);
when 12 => return to_signed(41, angle_t'length);
when 13 => return to_signed(20, angle_t'length);
when 14 => return to_signed(10, angle_t'length);
when others => return to_signed(5, angle_t'length);
end case;
end function;
function absolute_coordinate(value : coordinate_t) return coordinate_t is
begin
if value < 0 then
return -value;
end if;
return value;
end function;
function round_phase(value : angle_t) return phase_t is
variable rounded : angle_t;
begin
if value >= 0 then
rounded := shift_right(value + 8, 4);
else
rounded := -shift_right(-value + 8, 4);
end if;
if rounded >= 32768 then
rounded := rounded - 65536;
elsif rounded < -32768 then
rounded := rounded + 65536;
end if;
return resize(rounded, phase_t'length);
end function;
function round_magnitude(value : product_t) return magnitude_t is
variable rounded : product_t;
begin
rounded := shift_right(value + 524288, 20);
if rounded > 46341 then
return to_unsigned(46341, magnitude_t'length);
end if;
return resize(unsigned(rounded), magnitude_t'length);
end function;
function gain_term(value : coordinate_t; index : natural) return product_t is
variable widened : product_t;
begin
widened := resize(value, product_t'length);
case index is
when 0 => return shift_left(widened, 2);
when 1 => return shift_left(widened, 4);
when 2 => return -shift_left(widened, 6);
when 3 => return -shift_left(widened, 9);
when 4 => return -shift_left(widened, 12);
when 5 => return shift_left(widened, 15);
when others => return shift_left(widened, 17);
end case;
end function;
begin
ctor_phase <= resize(phase_result, ctor_phase'length);
ctor_magnitude <= resize(magnitude_result, ctor_magnitude'length);
process (ctor_lvt_context_in.clk)
variable input_x, input_y : signed(15 downto 0);
variable widened_x, widened_y : coordinate_t;
variable old_x, old_y : coordinate_t;
variable shifted_x, shifted_y : coordinate_t;
variable angle : angle_t;
variable term, next_accumulator : product_t;
begin
if rising_edge(ctor_lvt_context_in.clk) then
if ctor_lvt_context_in.rst = '1' then
worker_state <= idle;
pending <= '0';
x <= (others => '0');
y <= (others => '0');
accumulated_phase <= (others => '0');
normalization <= 0;
normalize_step <= 0;
iteration <= 0;
denormalize_step <= 0;
gain_step <= 0;
gain_accumulator <= (others => '0');
phase_result <= (others => '0');
magnitude_result <= (others => '0');
ctor_acknowledge <= '0';
ctor_success <= false;
elsif worker_state = normalize then
if absolute_coordinate(x) < 65536
and absolute_coordinate(y) < 65536 then
x <= shift_left(x, 1);
y <= shift_left(y, 1);
normalization <= normalization + 1;
end if;
if normalize_step = 13 then
iteration <= 0;
worker_state <= vector;
else
normalize_step <= normalize_step + 1;
end if;
elsif worker_state = vector then
old_x := x;
old_y := y;
shifted_x := shift_right(old_x, iteration);
shifted_y := shift_right(old_y, iteration);
angle := atan_phase(iteration);
if old_y >= 0 then
x <= old_x + shifted_y;
y <= old_y - shifted_x;
accumulated_phase <= accumulated_phase + angle;
else
x <= old_x - shifted_y;
y <= old_y + shifted_x;
accumulated_phase <= accumulated_phase - angle;
end if;
if iteration = 15 then
worker_state <= capture_phase;
else
iteration <= iteration + 1;
end if;
elsif worker_state = capture_phase then
phase_result <= round_phase(accumulated_phase);
denormalize_step <= 0;
worker_state <= denormalize;
elsif worker_state = denormalize then
if denormalize_step < normalization then
x <= shift_right(x, 1);
end if;
if denormalize_step = 13 then
gain_accumulator <= (others => '0');
gain_step <= 0;
worker_state <= scale_gain;
else
denormalize_step <= denormalize_step + 1;
end if;
elsif worker_state = scale_gain then
term := gain_term(x, gain_step);
next_accumulator := gain_accumulator + term;
gain_accumulator <= next_accumulator;
if gain_step = 6 then
magnitude_result <= round_magnitude(next_accumulator);
worker_state <= finish;
else
gain_step <= gain_step + 1;
end if;
elsif worker_state = finish then
ctor_success <= true;
ctor_acknowledge <= pending;
worker_state <= idle;
elsif ctor_request /= ctor_acknowledge then
pending <= ctor_request;
phase_result <= (others => '0');
magnitude_result <= (others => '0');
ctor_success <= false;
if not ctor_valid then
ctor_acknowledge <= ctor_request;
else
input_x := signed(ctor_xbits);
input_y := signed(ctor_ybits);
widened_x := shift_left(resize(input_x, x'length), 2);
widened_y := shift_left(resize(input_y, y'length), 2);
if input_x < 0 then
x <= -widened_x;
y <= -widened_y;
if input_y >= 0 then
accumulated_phase <= to_signed(524288, angle_t'length);
else
accumulated_phase <= to_signed(-524288, angle_t'length);
end if;
else
x <= widened_x;
y <= widened_y;
accumulated_phase <= (others => '0');
end if;
normalization <= 0;
normalize_step <= 0;
worker_state <= normalize;
end if;
end if;
end if;
end process;
end;