Added filtering function to PSS

lib/include/srslte/phy/sync/pss.h

@@ -71,8 +71,6 @@
 /* Low-level API */
 typedef struct SRSLTE_API {
 
-  srslte_dft_plan_t dftp_input; 
-  
 #ifdef CONVOLUTION_FFT
   srslte_conv_fft_cc_t conv_fft;
   srslte_filt_cc_t filter;
@@ -97,6 +95,13 @@ typedef struct SRSLTE_API {
   float ema_alpha;
   float *conv_output_avg;
   float peak_value;
+
+  bool filter_pss_enable;
+  srslte_dft_plan_t dftp_input;
+  srslte_dft_plan_t idftp_input;
+  cf_t tmp_fft[SRSLTE_SYMBOL_SZ_MAX];
+  cf_t tmp_fft2[SRSLTE_SYMBOL_SZ_MAX];
+
 }srslte_pss_synch_t;
 
 typedef enum { PSS_TX, PSS_RX } pss_direction_t;
@@ -128,6 +133,13 @@ SRSLTE_API void srslte_pss_synch_free(srslte_pss_synch_t *q);
 
 SRSLTE_API void srslte_pss_synch_reset(srslte_pss_synch_t *q); 
 
+SRSLTE_API void srslte_pss_synch_filter_enable(srslte_pss_synch_t *q,
+                                               bool enable);
+
+SRSLTE_API void srslte_pss_synch_filter(srslte_pss_synch_t *q,
+                                        cf_t *input,
+                                        cf_t *output);
+
 SRSLTE_API int srslte_pss_generate(cf_t *signal, 
                                    uint32_t N_id_2);
 

lib/src/phy/sync/pss.c

@@ -30,12 +30,8 @@
 #include <stdlib.h>
 #include <complex.h>
 #include <math.h>
-#include <srslte/phy/sync/pss.h>
 
 #include "srslte/phy/sync/pss.h"
-#include "srslte/phy/dft/dft.h"
-#include "srslte/phy/utils/vector.h"
-#include "srslte/phy/utils/convolution.h"
 #include "srslte/phy/utils/debug.h"
 
 
@@ -123,6 +119,8 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
 
     buffer_size = fft_size + frame_size + 1;
 
+    q->filter_pss_enable = true;
+
     if(q->decimate > 1) {
         int filter_order = 3;
         srslte_filt_decim_cc_init(&q->filter,q->decimate,filter_order);
@@ -137,7 +135,17 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
     }
     srslte_dft_plan_set_mirror(&q->dftp_input, true);
     srslte_dft_plan_set_dc(&q->dftp_input, true);
-    srslte_dft_plan_set_norm(&q->dftp_input, true);
+    srslte_dft_plan_set_norm(&q->dftp_input, false);
+
+    if (srslte_dft_plan(&q->idftp_input, fft_size, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
+      fprintf(stderr, "Error creating DFT plan \n");
+      goto clean_and_exit;
+    }
+    srslte_dft_plan_set_mirror(&q->idftp_input, true);
+    srslte_dft_plan_set_dc(&q->idftp_input, true);
+    srslte_dft_plan_set_norm(&q->idftp_input, true);
+
+    bzero(q->tmp_fft2, sizeof(cf_t)*SRSLTE_SYMBOL_SZ_MAX);
 
     q->tmp_input = srslte_vec_malloc((buffer_size + frame_size*(q->decimate - 1)) * sizeof(cf_t));
     if (!q->tmp_input) {
@@ -248,6 +256,13 @@ int srslte_pss_synch_resize(srslte_pss_synch_t *q, uint32_t frame_size, uint32_t
       return SRSLTE_ERROR;
     }
 
+    if (srslte_dft_replan(&q->idftp_input, fft_size)) {
+      fprintf(stderr, "Error creating DFT plan \n");
+      return SRSLTE_ERROR;
+    }
+
+    bzero(q->tmp_fft2, sizeof(cf_t)*SRSLTE_SYMBOL_SZ_MAX);
+
     bzero(&q->tmp_input[q->frame_size], q->fft_size * sizeof(cf_t));
     bzero(q->conv_output, sizeof(cf_t) * buffer_size);
     bzero(q->conv_output_avg, sizeof(float) * buffer_size);
@@ -314,6 +329,7 @@ void srslte_pss_synch_free(srslte_pss_synch_t *q) {
     }
 
     srslte_dft_plan_free(&q->dftp_input);
+    srslte_dft_plan_free(&q->idftp_input);
 
     if(q->decimate > 1)
     {
@@ -560,19 +576,35 @@ int srslte_pss_synch_chest(srslte_pss_synch_t *q, cf_t *input, cf_t ce[SRSLTE_PS
   return ret;
 }
 
+// Frequency-domain filtering of the central 64 sub-carriers
+void srslte_pss_synch_filter(srslte_pss_synch_t *q, cf_t *input, cf_t *output)
+{
+  srslte_dft_run_c(&q->dftp_input, input, q->tmp_fft);
+
+  memcpy(&q->tmp_fft2[q->fft_size/2-SRSLTE_PSS_LEN/2],
+         &q->tmp_fft[q->fft_size/2-SRSLTE_PSS_LEN/2],
+         sizeof(cf_t)*SRSLTE_PSS_LEN);
+
+  srslte_dft_run_c(&q->idftp_input, q->tmp_fft2, output);
+}
+
 /* Returns the CFO estimation given a PSS received sequence
  *
  * Source: An Efficient CFO Estimation Algorithm for the Downlink of 3GPP-LTE
  *       Feng Wang and Yu Zhu
  */
 float srslte_pss_synch_cfo_compute(srslte_pss_synch_t* q, cf_t *pss_recv) {
-  cf_t y0, y1, yr;
+  cf_t y0, y1;
 
-  y0 = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], pss_recv, q->fft_size/2);
-  y1 = srslte_vec_dot_prod_ccc(&q->pss_signal_time[q->N_id_2][q->fft_size/2], &pss_recv[q->fft_size/2], q->fft_size/2);
+  cf_t *pss_ptr = pss_recv;
 
-  yr = conjf(y0) * y1;
+  if (q->filter_pss_enable) {
+    srslte_pss_synch_filter(q, pss_recv, q->tmp_fft);
+    pss_ptr = q->tmp_fft;
+  }
 
-  return atan2f(__imag__ yr, __real__ yr) / M_PI;
+  y0 = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], pss_ptr, q->fft_size/2);
+  y1 = srslte_vec_dot_prod_ccc(&q->pss_signal_time[q->N_id_2][q->fft_size/2], &pss_ptr[q->fft_size/2], q->fft_size/2);
+  return carg(conjf(y0) * y1)/M_PI;
 }