Add func_sdp_subband_equalizer().

applications/lofar2/libraries/sdp/tb/vhdl/tb_sdp_pkg.vhd

@@ -77,6 +77,19 @@ PACKAGE tb_sdp_pkg is
 
   FUNCTION func_sdp_verify_cep_header(in_hdr, exp_hdr : t_sdp_cep_header) RETURN BOOLEAN;
 
+  -----------------------------------------------------------------------------
+  -- Subband equalizer (ESub)
+  -----------------------------------------------------------------------------
+  -- - ESub is like a two input beamformer
+  -- - Esub function calculates the expected subband level for signal input sp:
+  --   sp phasor * sp_weight + cross_phasor * cross weigth, where
+  --   . sp phasor = (sp_subband_ampl, sp_subband_phase)
+  --   . sp_weight = (sp_esub_gain, sp_esub_phase)
+  --   . cross_phasor = (cross_subband_ampl, cross_subband_phase)
+  --   . cross_weight = (cross_esub_gain, cross_esub_phase)
+  FUNCTION func_sdp_subband_equalizer(sp_subband_ampl, sp_subband_phase, sp_esub_gain, sp_esub_phase,
+                                      cross_subband_ampl, cross_subband_phase, cross_esub_gain, cross_esub_phase : REAL)
+                                      RETURN t_real_arr;  -- 0:3 = ampl, phase, re, im
 
 END PACKAGE tb_sdp_pkg;
 
@@ -393,4 +406,30 @@ PACKAGE BODY tb_sdp_pkg IS
     RETURN TRUE;
   END func_sdp_verify_cep_header;
 
+
+  FUNCTION func_sdp_subband_equalizer(sp_subband_ampl, sp_subband_phase, sp_esub_gain, sp_esub_phase,
+                                      cross_subband_ampl, cross_subband_phase, cross_esub_gain, cross_esub_phase : REAL)
+                                      RETURN t_real_arr IS  -- 0:3 = ampl, phase, re, im
+    VARIABLE v_sp_ampl, v_sp_phase, v_sp_re, v_sp_im : REAL;
+    VARIABLE v_cross_ampl, v_cross_phase, v_cross_re, v_cross_im : REAL;
+    VARIABLE v_sum_ampl, v_sum_phase, v_sum_re, v_sum_im : REAL;
+    VARIABLE v_tuple : t_real_arr(0 TO 3);
+  BEGIN
+    v_sp_ampl   := sp_subband_ampl * sp_esub_gain;
+    v_sp_phase  := sp_subband_phase + sp_esub_phase;
+    v_sp_re     := COMPLEX_RE(v_sp_ampl, v_sp_phase);
+    v_sp_im     := COMPLEX_IM(v_sp_ampl, v_sp_phase);
+    v_cross_ampl  := cross_subband_ampl * cross_esub_gain;
+    v_cross_phase := cross_subband_phase + cross_esub_phase;
+    v_cross_re    := COMPLEX_RE(v_cross_ampl, v_cross_phase);
+    v_cross_im    := COMPLEX_IM(v_cross_ampl, v_cross_phase);
+    v_sum_re    := v_sp_re + v_cross_re;  -- ESub sum re
+    v_sum_im    := v_sp_im + v_cross_im;  -- ESub sum im
+    v_sum_ampl  := COMPLEX_RADIUS(v_sum_re, v_sum_im);
+    v_sum_phase := COMPLEX_PHASE(v_sum_re, v_sum_im);
+    v_tuple     := (0 => v_sum_ampl, 1 => v_sum_phase, 2 => v_sum_re, 3 => v_sum_im);
+    RETURN v_tuple;
+  END;
+
+
 END tb_sdp_pkg;