Add ffo compensation based on SSS

openair1/PHY/NR_REFSIG/sss_nr.h

@@ -96,7 +96,7 @@ int pss_ch_est_nr(PHY_VARS_NR_UE *ue,
                   int32_t pss_ext[NB_ANTENNAS_RX][LENGTH_PSS_NR],
                   int32_t sss_ext[NB_ANTENNAS_RX][LENGTH_SSS_NR]);
 
-int rx_sss_nr(PHY_VARS_NR_UE *ue, UE_nr_rxtx_proc_t *proc, int32_t *tot_metric, uint8_t *phase_max);
+int rx_sss_nr(PHY_VARS_NR_UE *ue, UE_nr_rxtx_proc_t *proc, int32_t *tot_metric, uint8_t *phase_max, int *freq_offset_sss);
 
 #undef INIT_VARIABLES_SSS_NR_H
 #undef EXTERN

openair1/PHY/NR_UE_TRANSPORT/nr_initial_sync.c

@@ -295,7 +295,34 @@ int nr_initial_sync(UE_nr_rxtx_proc_t *proc,
       LOG_I(PHY,"Calling sss detection (normal CP)\n");
 #endif
 
-      rx_sss_nr(ue, proc, &metric_tdd_ncp, &phase_tdd_ncp);
+      int freq_offset_sss = 0;
+      rx_sss_nr(ue, proc, &metric_tdd_ncp, &phase_tdd_ncp, &freq_offset_sss);
+
+      accumulated_freq_offset += freq_offset_sss;
+
+      // digital compensation of FFO for SSB symbols
+      if (ue->UE_fo_compensation){
+        double s_time = 1/(1.0e3*fp->samples_per_subframe);  // sampling time
+        double off_angle = -2*M_PI*s_time*freq_offset_sss;   // offset rotation angle compensation per sample
+
+        // In SA we need to perform frequency offset correction until the end of buffer because we need to decode SIB1
+        // and we do not know yet in which slot it goes.
+
+        // start for offset correction
+        int start = sa ? is*fp->samples_per_frame : is*fp->samples_per_frame + ue->ssb_offset;
+
+        // loop over samples
+        int end = sa ? n_frames*fp->samples_per_frame-1 : start + NR_N_SYMBOLS_SSB*(fp->ofdm_symbol_size + fp->nb_prefix_samples);
+
+        for(int n=start; n<end; n++){
+          for (int ar=0; ar<fp->nb_antennas_rx; ar++) {
+            re = ((double)(((short *)ue->common_vars.rxdata[ar]))[2*n]);
+            im = ((double)(((short *)ue->common_vars.rxdata[ar]))[2*n+1]);
+            ((short *)ue->common_vars.rxdata[ar])[2*n] = (short)(round(re*cos(n*off_angle) - im*sin(n*off_angle)));
+            ((short *)ue->common_vars.rxdata[ar])[2*n+1] = (short)(round(re*sin(n*off_angle) + im*cos(n*off_angle)));
+          }
+        }
+      }
 
       nr_gold_pbch(ue);
       ret = nr_pbch_detection(proc, ue, 1);  // start pbch detection at first symbol after pss

openair1/PHY/NR_UE_TRANSPORT/sss_nr.c

@@ -420,7 +420,7 @@ int pss_sss_extract_nr(PHY_VARS_NR_UE *phy_vars_ue,
 *
 *********************************************************************/
 
-int rx_sss_nr(PHY_VARS_NR_UE *ue, UE_nr_rxtx_proc_t *proc, int32_t *tot_metric, uint8_t *phase_max)
+int rx_sss_nr(PHY_VARS_NR_UE *ue, UE_nr_rxtx_proc_t *proc, int32_t *tot_metric, uint8_t *phase_max, int *freq_offset_sss)
 {
   uint8_t i;
   int32_t pss_ext[NB_ANTENNAS_RX][LENGTH_PSS_NR];
@@ -558,5 +558,19 @@ int rx_sss_nr(PHY_VARS_NR_UE *ue, UE_nr_rxtx_proc_t *proc, int32_t *tot_metric,
   }
   //#endif
 
+  int re = 0;
+  int im = 0;
+  d = (int16_t *)&d_sss[Nid2][Nid1];
+  for(i = 0; i<LENGTH_SSS_NR; i++) {
+    re += d[i]*sss[2*i];
+    im += d[i]*sss[2*i+1];
+  }
+  double ffo_sss = atan2(im,re)/M_PI/4.3;
+  *freq_offset_sss = (int)(ffo_sss*frame_parms->subcarrier_spacing);
+
+  double ffo_pss = ((double)ue->common_vars.freq_offset)/frame_parms->subcarrier_spacing;
+  LOG_I(NR_PHY, "ffo_pss %f (%i Hz), ffo_sss %f (%i Hz),  ffo_pss+ffo_sss %f (%i Hz)\n",
+         ffo_pss, (int)(ffo_pss*frame_parms->subcarrier_spacing), ffo_sss, *freq_offset_sss, ffo_pss+ffo_sss, (int)((ffo_pss+ffo_sss)*frame_parms->subcarrier_spacing));
+
   return(0);
 }