Add noise reduction in PSS before estimating CFO

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,12 @@ typedef struct SRSLTE_API {
   float ema_alpha;
   float *conv_output_avg;
   float peak_value;
+
+  srslte_dft_plan_t dftp_input;
+  srslte_dft_plan_t idftp_input;
+  cf_t tmp_fft[SRSLTE_SYMBOL_SZ_MAX];
+  cf_t yr[SRSLTE_PSS_LEN];
+
 }srslte_pss_synch_t;
 
 typedef enum { PSS_TX, PSS_RX } pss_direction_t;

lib/src/phy/sync/pss.c

@@ -137,7 +137,13 @@ 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);
+
+    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);
 
     q->tmp_input = srslte_vec_malloc((buffer_size + frame_size*(q->decimate - 1)) * sizeof(cf_t));
     if (!q->tmp_input) {
@@ -314,6 +320,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 +567,34 @@ int srslte_pss_synch_chest(srslte_pss_synch_t *q, cf_t *input, cf_t ce[SRSLTE_PS
   return ret;
 }
 
+#define CLEAN_NOISE_DFT
+//#define PSS_CFO_FREQ
 /* 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);
+#ifdef PSS_CFO_FREQ
 
-  yr = conjf(y0) * y1;
+  srslte_vec_prod_conj_ccc(&q->tmp_fft[q->fft_size/2-SRSLTE_PSS_LEN/2], q->pss_signal_freq[q->N_id_2], q->yr, SRSLTE_PSS_LEN);
+  y0 = srslte_vec_acc_cc(q->yr, SRSLTE_PSS_LEN/2);
+  y1 = srslte_vec_acc_cc(&q->yr[SRSLTE_PSS_LEN/2], SRSLTE_PSS_LEN/2);
 
-  return atan2f(__imag__ yr, __real__ yr) / M_PI;
+#else
+#ifdef CLEAN_NOISE_DFT
+  // Eliminate noise
+  srslte_dft_run_c(&q->dftp_input, pss_recv, q->tmp_fft);
+  bzero(q->tmp_fft,                                  sizeof(cf_t)*(q->fft_size/2-SRSLTE_PSS_LEN/2));
+  bzero(&q->tmp_fft[q->fft_size/2+SRSLTE_PSS_LEN/2], sizeof(cf_t)*(q->fft_size/2-SRSLTE_PSS_LEN/2));
+  srslte_dft_run_c(&q->idftp_input, q->tmp_fft, pss_recv);
+#endif
+
+  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);
+#endif
+  return carg(conjf(y0) * y1)/M_PI;
 }