Merge branch 'nr-cfo-estimation' into 'develop-nr'

Nr cfo estimation

See merge request oai/openairinterface5g!499

openair1/PHY/NR_REFSIG/pss_nr.h

@@ -137,7 +137,9 @@ int set_pss_nr(int ofdm_symbol_size);
 int pss_synchro_nr(PHY_VARS_NR_UE *PHY_vars_UE, int rate_change);
 int pss_search_time_nr(int **rxdata, ///rx data in time domain
                        NR_DL_FRAME_PARMS *frame_parms,
-                       int *eNB_id);
+		       int fo_flag,
+                       int *eNB_id,
+		       int *f_off);
 
 #endif
 #undef EXTERN

openair1/PHY/NR_UE_TRANSPORT/nr_initial_sync.c

@@ -39,6 +39,7 @@
 //#include "SCHED/extern.h"
 
 #include "common_lib.h"
+#include <math.h>
 
 #include "PHY/NR_REFSIG/pss_nr.h"
 #include "PHY/NR_REFSIG/sss_nr.h"
@@ -56,6 +57,7 @@ int nr_pbch_detection(PHY_VARS_NR_UE *ue, runmode_t mode)
   NR_DL_FRAME_PARMS *frame_parms=&ue->frame_parms;
   int ret =-1;
 
+
 #ifdef DEBUG_INITIAL_SYNCH
   LOG_I(PHY,"[UE%d] Initial sync: starting PBCH detection (rx_offset %d)\n",ue->Mod_id,
         ue->rx_offset);
@@ -65,6 +67,7 @@ int nr_pbch_detection(PHY_VARS_NR_UE *ue, runmode_t mode)
   int nb_prefix_samples0 = frame_parms->nb_prefix_samples0;
   frame_parms->nb_prefix_samples0 = frame_parms->nb_prefix_samples;
 
+
   //symbol 1
   nr_slot_fep(ue,
 	      1,
@@ -143,6 +146,7 @@ int nr_initial_sync(PHY_VARS_NR_UE *ue, runmode_t mode)
   int32_t sync_pos, sync_pos_slot; // k_ssb, N_ssb_crb, sync_pos2,
   int32_t metric_tdd_ncp=0;
   uint8_t phase_tdd_ncp;
+  double im, re;
 
   NR_DL_FRAME_PARMS *fp = &ue->frame_parms;
   int ret=-1;
@@ -183,14 +187,14 @@ int nr_initial_sync(PHY_VARS_NR_UE *ue, runmode_t mode)
   sync_pos = pss_synchro_nr(ue, NO_RATE_CHANGE);
 
   sync_pos_slot = (fp->samples_per_subframe/fp->slots_per_subframe) - 10*(fp->ofdm_symbol_size + fp->nb_prefix_samples);
-  
+
   if (sync_pos >= fp->nb_prefix_samples){
     ue->ssb_offset = sync_pos - fp->nb_prefix_samples;}
   else{
     ue->ssb_offset = sync_pos + (fp->samples_per_subframe * 10) - fp->nb_prefix_samples;}
     
-    ue->rx_offset = ue->ssb_offset - sync_pos_slot;
-  
+  ue->rx_offset = ue->ssb_offset - sync_pos_slot;
+
   //write_output("rxdata1.m","rxd1",ue->common_vars.rxdata[0],10*fp->samples_per_subframe,1,1);
 
 #ifdef DEBUG_INITIAL_SYNCH
@@ -198,6 +202,25 @@ int nr_initial_sync(PHY_VARS_NR_UE *ue, runmode_t mode)
   LOG_I(PHY,"sync_pos %d ssb_offset %d sync_pos_slot %d \n",sync_pos,ue->ssb_offset,sync_pos_slot);
 #endif
 
+  // 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*(ue->common_vars.freq_offset);  // offset rotation angle compensation per sample
+
+	int start = ue->ssb_offset;  // start for offset correction is at ssb_offset (pss time position)
+  	int end = start + 4*(fp->ofdm_symbol_size + fp->nb_prefix_samples);  // loop over samples in 4 symbols (ssb size), including prefix  
+
+	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)));
+	  }
+	}
+  }
+
+
   /* check that SSS/PBCH block is continuous inside the received buffer */
   if (sync_pos < (NR_NUMBER_OF_SUBFRAMES_PER_FRAME*fp->samples_per_subframe - (NB_SYMBOLS_PBCH * fp->ofdm_symbol_size))) {
 
@@ -244,7 +267,8 @@ int nr_initial_sync(PHY_VARS_NR_UE *ue, runmode_t mode)
 	  ret = -1;
   }
 
-  /* Consider this is a false detection if the offset is > 1000 Hz */
+  /* Consider this is a false detection if the offset is > 1000 Hz 
+     Not to be used now that offest estimation is in place
   if( (abs(ue->common_vars.freq_offset) > 150) && (ret == 0) )
   {
 	  ret=-1;
@@ -253,7 +277,7 @@ int nr_initial_sync(PHY_VARS_NR_UE *ue, runmode_t mode)
 #else
 	  LOG_E(HW, "Ignore MIB with high freq offset [%d Hz] estimation \n",ue->common_vars.freq_offset);
 #endif
-  }
+  }*/
 
   if (ret==0) {  // PBCH found so indicate sync to higher layers and configure frame parameters
 
@@ -329,13 +353,13 @@ int nr_initial_sync(PHY_VARS_NR_UE *ue, runmode_t mode)
     printf("[UE %d] Frame %d Measured Carrier Frequency %.0f Hz (offset %d Hz)\n",
 	  ue->Mod_id,
 	  ue->proc.proc_rxtx[0].frame_rx,
-	  openair0_cfg[0].rx_freq[0]-ue->common_vars.freq_offset,
+	  openair0_cfg[0].rx_freq[0]+ue->common_vars.freq_offset,
 	  ue->common_vars.freq_offset);
 #  else
     LOG_I(PHY, "[UE %d] Frame %d Measured Carrier Frequency %.0f Hz (offset %d Hz)\n",
 	  ue->Mod_id,
 	  ue->proc.proc_rxtx[0].frame_rx,
-	  openair0_cfg[0].rx_freq[0]-ue->common_vars.freq_offset,
+	  openair0_cfg[0].rx_freq[0]+ue->common_vars.freq_offset,
 	  ue->common_vars.freq_offset);
 #  endif
 #endif

openair1/PHY/NR_UE_TRANSPORT/pss_nr.c

@@ -33,6 +33,7 @@
 #include <stdio.h>
 #include <assert.h>
 #include <errno.h>
+#include <math.h>
 
 #include "PHY/defs_nr_UE.h"
 
@@ -662,6 +663,7 @@ int pss_synchro_nr(PHY_VARS_NR_UE *PHY_vars_UE, int rate_change)
   NR_DL_FRAME_PARMS *frame_parms = &(PHY_vars_UE->frame_parms);
   int synchro_position;
   int **rxdata = NULL;
+  int fo_flag = PHY_vars_UE->UE_fo_compensation;  // flag to enable freq offset estimation and compensation
 
 #ifdef DBG_PSS_NR
 
@@ -705,7 +707,10 @@ int pss_synchro_nr(PHY_VARS_NR_UE *PHY_vars_UE, int rate_change)
 
   synchro_position = pss_search_time_nr(rxdata,
                                         frame_parms,
-                                        (int *)&PHY_vars_UE->common_vars.eNb_id);
+					fo_flag,
+                                        (int *)&PHY_vars_UE->common_vars.eNb_id,
+					(int *)&PHY_vars_UE->common_vars.freq_offset);
+
 
 #if TEST_SYNCHRO_TIMING_PSS
 
@@ -751,6 +756,15 @@ static inline int64_t abs64(int64_t x)
   return (((int64_t)((int32_t*)&x)[0])*((int64_t)((int32_t*)&x)[0]) + ((int64_t)((int32_t*)&x)[1])*((int64_t)((int32_t*)&x)[1]));
 }
 
+static inline double angle64(int64_t x)
+{
+
+  double re=((int32_t*)&x)[0];
+  double im=((int32_t*)&x)[1];
+  return (atan2(im,re));
+}
+
+
 /*******************************************************************
 *
 * NAME :         pss_search_time_nr
@@ -805,12 +819,15 @@ static inline int64_t abs64(int64_t x)
 
 int pss_search_time_nr(int **rxdata, ///rx data in time domain
                        NR_DL_FRAME_PARMS *frame_parms,
-                       int *eNB_id)
+		       int fo_flag,
+                       int *eNB_id,
+		       int *f_off)
 {
   unsigned int n, ar, peak_position, pss_source;
   int64_t peak_value;
   int64_t result;
   int64_t avg[NUMBER_PSS_SEQUENCE];
+  double ffo_est=0;
 
 
   unsigned int length = (NR_NUMBER_OF_SUBFRAMES_PER_FRAME*frame_parms->samples_per_subframe);  /* 1 frame for now, it should be 2 TODO_NR */
@@ -831,7 +848,6 @@ int pss_search_time_nr(int **rxdata, ///rx data in time domain
     maxval = max(maxval,-primary_synchro_time_nr[1][i]);
     maxval = max(maxval,primary_synchro_time_nr[2][i]);
     maxval = max(maxval,-primary_synchro_time_nr[2][i]);
-
   }
   int shift = log2_approx(maxval);//*(frame_parms->ofdm_symbol_size+frame_parms->nb_prefix_samples)*2);
 
@@ -859,7 +875,6 @@ int pss_search_time_nr(int **rxdata, ///rx data in time domain
 				  frame_parms->ofdm_symbol_size, 
 				  shift);
 	  pss_corr_ue[pss_index][n] += abs64(result);
-	  
           //((short*)pss_corr_ue[pss_index])[2*n] += ((short*) &result)[0];   /* real part */
           //((short*)pss_corr_ue[pss_index])[2*n+1] += ((short*) &result)[1]; /* imaginary part */
           //((short*)&synchro_out)[0] += ((int*) &result)[0];               /* real part */
@@ -867,7 +882,7 @@ int pss_search_time_nr(int **rxdata, ///rx data in time domain
 
         }
       }
-
+ 
       /* calculate the absolute value of sync_corr[n] */
       avg[pss_index]+=pss_corr_ue[pss_index][n];
       if (pss_corr_ue[pss_index][n] > peak_value) {
@@ -882,11 +897,45 @@ int pss_search_time_nr(int **rxdata, ///rx data in time domain
     }
   }
 
+  if (fo_flag){
+	  // fractional frequency offser computation according to Cross-correlation Synchronization Algorithm Using PSS
+	  // Shoujun Huang, Yongtao Su, Ying He and Shan Tang, "Joint time and frequency offset estimation in LTE downlink," 7th International Conference on Communications and Networking in China, 2012.
+
+	  int64_t result1,result2;
+	  // Computing cross-correlation at peak on half the symbol size for first half of data
+	  result1  = dot_product64((short*)primary_synchro_time_nr[pss_source], 
+				  (short*) &(rxdata[0][peak_position]), 
+				  frame_parms->ofdm_symbol_size>>1, 
+				  shift);
+	  // Computing cross-correlation at peak on half the symbol size for data shifted by half symbol size 
+	  // as it is real and complex it is necessary to shift by a value equal to symbol size to obtain such shift
+	  result2  = dot_product64((short*)primary_synchro_time_nr[pss_source]+(frame_parms->ofdm_symbol_size), 
+				  (short*) &(rxdata[0][peak_position])+(frame_parms->ofdm_symbol_size), 
+				  frame_parms->ofdm_symbol_size>>1, 
+				  shift);
+
+	  int64_t re1,re2,im1,im2;
+	  re1=((int*) &result1)[0];
+	  re2=((int*) &result2)[0];
+	  im1=((int*) &result1)[1];
+	  im2=((int*) &result2)[1];
+
+ 	  // estimation of fractional frequency offset: angle[(result1)'*(result2)]/pi
+	  ffo_est=atan2(re1*im2-re2*im1,re1*re2+im1*im2)/M_PI;
+  
+#ifdef DBG_PSS_NR
+	  printf("ffo %lf\n",ffo_est);
+#endif
+  }
+
+  // computing absolute value of frequency offset
+  *f_off = ffo_est*frame_parms->subcarrier_spacing;  
+
   for (int pss_index = 0; pss_index < NUMBER_PSS_SEQUENCE; pss_index++) avg[pss_index]/=(length/4);
 
   *eNB_id = pss_source;
 
-  LOG_I(PHY,"[UE] nr_synchro_time: Sync source = %d, Peak found at pos %d, val = %llu (%d dB) avg %d dB\n", pss_source, peak_position, (unsigned long long)peak_value, dB_fixed64(peak_value),dB_fixed64(avg[pss_source]));
+  LOG_I(PHY,"[UE] nr_synchro_time: Sync source = %d, Peak found at pos %d, val = %llu (%d dB) avg %d dB, ffo %lf\n", pss_source, peak_position, (unsigned long long)peak_value, dB_fixed64(peak_value),dB_fixed64(avg[pss_source]),ffo_est);
 
   if (peak_value < 5*avg[pss_source])
     return(-1);

openair1/PHY/TOOLS/cdot_prod.c

@@ -169,7 +169,7 @@ int64_t dot_product64(int16_t *x,
 #if defined(__x86_64__) || defined(__i386__)
   __m128i *x128,*y128,mmtmp1,mmtmp2,mmtmp3,mmcumul,mmcumul_re,mmcumul_im;
   __m128i minus_i = _mm_set_epi16(-1,1,-1,1,-1,1,-1,1);
-  int32_t result;
+  int64_t result;
 
   x128 = (__m128i*) x;
   y128 = (__m128i*) y;
@@ -180,14 +180,13 @@ int64_t dot_product64(int16_t *x,
   for (n=0; n<(N>>2); n++) {
 
 //    printf("n=%d, x128=%p, y128=%p\n",n,x128,y128);
-  //      print_shorts("x",&x128[0]);
-    //    print_shorts("y",&y128[0]);
+       // print_shorts("x",&x128[0]);
+       // print_shorts("y",&y128[0]);
 
     // this computes Re(z) = Re(x)*Re(y) + Im(x)*Im(y)
     mmtmp1 = _mm_madd_epi16(x128[0],y128[0]);
       //  print_ints("retmp",&mmtmp1);
     // mmtmp1 contains real part of 4 consecutive outputs (32-bit)
-
     // shift and accumulate results
     mmtmp1 = _mm_srai_epi32(mmtmp1,output_shift);
     mmcumul_re = _mm_add_epi32(mmcumul_re,mmtmp1);
@@ -205,7 +204,6 @@ int64_t dot_product64(int16_t *x,
     mmtmp3 = _mm_madd_epi16(x128[0],mmtmp2);
             //print_ints("imtmp",&mmtmp3);
     // mmtmp3 contains imag part of 4 consecutive outputs (32-bit)
-
     // shift and accumulate results
     mmtmp3 = _mm_srai_epi32(mmtmp3,output_shift);
     mmcumul_im = _mm_add_epi32(mmcumul_im,mmtmp3);
@@ -218,13 +216,10 @@ int64_t dot_product64(int16_t *x,
   // this gives Re Re Im Im
   mmcumul = _mm_hadd_epi32(mmcumul_re,mmcumul_im);
     //print_ints("cumul1",&mmcumul);
-
   // this gives Re Im Re Im
   mmcumul = _mm_hadd_epi32(mmcumul,mmcumul);
-
     //print_ints("cumul2",&mmcumul);
 
-
   //mmcumul = _mm_srai_epi32(mmcumul,output_shift);
   // extract the lower half
   result = _mm_extract_epi64(mmcumul,0);

openair1/PHY/defs_nr_UE.h

@@ -983,6 +983,8 @@ typedef struct {
   int UE_scan;
   /// \brief Indicator that UE should perform coarse scanning around carrier
   int UE_scan_carrier;
+  /// \brief Indicator that UE should enable estimation and compensation of frequency offset
+  int UE_fo_compensation;
   /// \brief Indicator that UE is synchronized to an eNB
   int is_synchronized;
   /// Data structure for UE process scheduling

openair1/SCHED_NR/phy_procedures_nr_gNB.c

@@ -138,20 +138,22 @@ void nr_common_signal_procedures (PHY_VARS_gNB *gNB,int frame, int slot) {
     LOG_D(PHY,"SS TX: frame %d, slot %d, start_symbol %d\n",frame,slot, ssb_start_symbol);
 
     nr_generate_pss(gNB->d_pss, txdataF[0], AMP, ssb_start_symbol, cfg, fp);
-    nr_generate_sss(gNB->d_sss, txdataF[0], AMP_OVER_2, ssb_start_symbol, cfg, fp);
+    nr_generate_sss(gNB->d_sss, txdataF[0], AMP, ssb_start_symbol, cfg, fp);
 
     if (!(frame&7)){
       LOG_D(PHY,"%d.%d : pbch_configured %d\n",frame,slot,gNB->pbch_configured);
       if (gNB->pbch_configured != 1)return;
       gNB->pbch_configured = 0;
     }
-    nr_generate_pbch_dmrs(gNB->nr_gold_pbch_dmrs[n_hf][ssb_index],txdataF[0], AMP_OVER_2, ssb_start_symbol, cfg, fp);
+
+    nr_generate_pbch_dmrs(gNB->nr_gold_pbch_dmrs[n_hf][ssb_index],txdataF[0], AMP, ssb_start_symbol, cfg, fp);
+
     nr_generate_pbch(&gNB->pbch,
                       gNB->nrPolar_params,
                       pbch_pdu,
                       gNB->nr_pbch_interleaver,
                       txdataF[0],
-                      AMP_OVER_2,
+                      AMP,
                       ssb_start_symbol,
                       n_hf,Lmax,ssb_index,
                       frame, cfg, fp);

openair1/SIMULATION/NR_PHY/pbchsim.c

@@ -87,11 +87,13 @@ int main(int argc, char **argv)
 
   int i,aa;//,l;
   double sigma2, sigma2_dB=10,SNR,snr0=-2.0,snr1=2.0;
+  double cfo=0;
   uint8_t snr1set=0;
   int **txdata;
   double **s_re,**s_im,**r_re,**r_im;
-  //double iqim = 0.0;
-  //unsigned char pbch_pdu[6];
+  double iqim = 0.0;
+  double ip =0.0;
+  unsigned char pbch_pdu[6];
   //  int sync_pos, sync_pos_slot;
   //  FILE *rx_frame_file;
   FILE *output_fd = NULL;
@@ -146,7 +148,7 @@ int main(int argc, char **argv)
 
   randominit(0);
 
-  while ((c = getopt (argc, argv, "f:hA:pf:g:i:j:n:s:S:t:x:y:z:N:F:GR:dP:IL:")) != -1) {
+  while ((c = getopt (argc, argv, "f:hA:pf:g:i:j:n:o:s:S:t:x:y:z:N:F:GR:dP:IL:")) != -1) {
     switch (c) {
     case 'f':
       write_output_file=1;
@@ -212,6 +214,11 @@ int main(int argc, char **argv)
       n_trials = atoi(optarg);
       break;
 
+    case 'o':
+      cfo = atof(optarg);
+      msg("Setting CFO to %f Hz\n",cfo);
+      break;
+
     case 's':
       snr0 = atof(optarg);
       msg("Setting SNR0 to %f\n",snr0);
@@ -325,6 +332,7 @@ int main(int argc, char **argv)
       printf("-z Number of RX antennas used in UE\n");
       printf("-i Relative strength of first intefering eNB (in dB) - cell_id mod 3 = 1\n");
       printf("-j Relative strength of second intefering eNB (in dB) - cell_id mod 3 = 2\n");
+      printf("-o Carrier frequency offset in Hz\n");
       printf("-N Nid_cell\n");
       printf("-R N_RB_DL\n");
       printf("-O oversampling factor (1,2,4,8,16)\n");
@@ -361,26 +369,44 @@ int main(int argc, char **argv)
   nr_phy_config_request_sim(gNB,N_RB_DL,N_RB_DL,mu,Nid_cell);
   phy_init_nr_gNB(gNB,0,0);
 
-  double fs,bw;
+  double fs,bw,scs,eps;
 
   if (mu == 1 && N_RB_DL == 217) { 
     fs = 122.88e6;
     bw = 80e6;
+    scs = 30000;
   }					       
   else if (mu == 1 && N_RB_DL == 245) {
     fs = 122.88e6;
     bw = 90e6;
+    scs = 30000;
   }
   else if (mu == 1 && N_RB_DL == 273) {
     fs = 122.88e6;
     bw = 100e6;
+    scs = 30000;
   }
   else if (mu == 1 && N_RB_DL == 106) { 
     fs = 61.44e6;
     bw = 40e6;
+    scs = 30000;
   }
   else AssertFatal(1==0,"Unsupported numerology for mu %d, N_RB %d\n",mu, N_RB_DL);
 
+  // cfo with respect to sub-carrier spacing
+  eps = cfo/scs;
+
+  // computation of integer and fractional FO to compare with estimation results
+  int IFO;
+  if(eps!=0.0){
+	printf("Introducing a CFO of %lf relative to SCS of %d kHz\n",eps,(int)(scs/1000));
+	if (eps>0)	
+  	  IFO=(int)(eps+0.5);
+	else
+	  IFO=(int)(eps-0.5);
+	printf("FFO = %lf; IFO = %d\n",eps-IFO,IFO);
+  }
+
   gNB2UE = new_channel_desc_scm(n_tx,
                                 n_rx,
                                 channel_model,
@@ -436,6 +462,9 @@ int main(int argc, char **argv)
                       
   UE->perfect_ce = 0;
 
+  if(eps!=0.0)
+	UE->UE_fo_compensation = 1; // if a frequency offset is set then perform fo estimation and compensation
+
   if (init_nr_ue_signal(UE, 1, 0) != 0)
   {
     printf("Error at UE NR initialisation\n");
@@ -493,6 +522,7 @@ int main(int argc, char **argv)
 
   //  printf("txlev %d (%f)\n",txlev,10*log10(txlev));
 
+
   for (i=0; i<frame_length_complex_samples; i++) {
     for (aa=0; aa<frame_parms->nb_antennas_tx; aa++) {
       r_re[aa][i] = ((double)(((short *)txdata[aa]))[(i<<1)]);
@@ -515,7 +545,28 @@ int main(int argc, char **argv)
       sigma2 = pow(10,sigma2_dB/10);
       //printf("sigma2 %f (%f dB), tx_lev %f (%f dB)\n",sigma2,sigma2_dB,txlev,10*log10((double)txlev));
 
-      for (i=0; i<frame_parms->samples_per_subframe; i++) {
+      if(eps!=0.0)
+        rf_rx(r_re,  // real part of txdata
+           r_im,  // imag part of txdata
+           NULL,  // interference real part
+           NULL, // interference imag part
+           0,  // interference power
+           frame_parms->nb_antennas_rx,  // number of rx antennas
+           frame_length_complex_samples,  // number of samples in frame
+           1.0e9/fs,   //sampling time (ns)
+           cfo,	// frequency offset in Hz
+           0.0, // drift (not implemented)
+           0.0, // noise figure (not implemented)
+           0.0, // rx gain in dB ?
+           200, // 3rd order non-linearity in dB ?
+           &ip, // initial phase
+           30.0e3,  // phase noise cutoff in kHz
+           -500.0, // phase noise amplitude in dBc
+           0.0,  // IQ imbalance (dB),
+	   0.0); // IQ phase imbalance (rad)
+
+   
+      for (i=0; i<frame_length_complex_samples; i++) {
 	for (aa=0; aa<frame_parms->nb_antennas_rx; aa++) {
 	  
 	  ((short*) UE->common_vars.rxdata[aa])[2*i]   = (short) ((r_re[aa][i] + sqrt(sigma2/2)*gaussdouble(0.0,1.0)));

openair1/SIMULATION/RF/rf.c

@@ -107,10 +107,10 @@ void rf_rx(double **r_re,
     exit(-1);
   }
 
-  if (fabs(f_off) > 10000.0) {
+/*  if (fabs(f_off) > 10000.0) {
     printf("rf.c: Illegal f_off %f\n",f_off);
     exit(-1);
-  }
+  }*/
 
   if (fabs(drift) > 1000.0) {
     printf("rf.c: Illegal drift %f\n",drift);

targets/RT/USER/nr-ue.c

@@ -444,6 +444,7 @@ static void *UE_thread_synch(void *arg) {
 
 	      //write_output("txdata_sym.m", "txdata_sym", UE->common_vars.rxdata[0], (10*UE->frame_parms.samples_per_subframe), 1, 1);
 
+		freq_offset = UE->common_vars.freq_offset; // frequency offset computed with pss in initial sync
                 hw_slot_offset = (UE->rx_offset<<1) / UE->frame_parms.samples_per_subframe;
                 printf("Got synch: hw_slot_offset %d, carrier off %d Hz, rxgain %d (DL %u, UL %u), UE_scan_carrier %d\n",
                        hw_slot_offset,
@@ -457,16 +458,13 @@ static void *UE_thread_synch(void *arg) {
                     // rerun with new cell parameters and frequency-offset
                     for (i=0; i<openair0_cfg[UE->rf_map.card].rx_num_channels; i++) {
                         openair0_cfg[UE->rf_map.card].rx_gain[UE->rf_map.chain+i] = UE->rx_total_gain_dB;//-USRP_GAIN_OFFSET;
-			if (UE->UE_scan_carrier == 1) {
 			  if (freq_offset >= 0)
-                            openair0_cfg[UE->rf_map.card].rx_freq[UE->rf_map.chain+i] += abs(UE->common_vars.freq_offset);
+                            openair0_cfg[UE->rf_map.card].rx_freq[UE->rf_map.chain+i] += abs(freq_offset);
 			  else
-                            openair0_cfg[UE->rf_map.card].rx_freq[UE->rf_map.chain+i] -= abs(UE->common_vars.freq_offset);
+                            openair0_cfg[UE->rf_map.card].rx_freq[UE->rf_map.chain+i] -= abs(freq_offset);
 			  openair0_cfg[UE->rf_map.card].tx_freq[UE->rf_map.chain+i] =
                             openair0_cfg[UE->rf_map.card].rx_freq[UE->rf_map.chain+i]+uplink_frequency_offset[CC_id][i];
 			  downlink_frequency[CC_id][i] = openair0_cfg[CC_id].rx_freq[i];
-			  freq_offset=0;
-			}
 		    }
 
                     // reconfigure for potentially different bandwidth

targets/RT/USER/nr-uesoftmodem.c

@@ -130,6 +130,7 @@ static char                    *itti_dump_file = NULL;
 
 int UE_scan = 0;
 int UE_scan_carrier = 0;
+int UE_fo_compensation = 0;
 runmode_t mode = normal_txrx;
 
 FILE *input_fd=NULL;
@@ -978,6 +979,7 @@ int main( int argc, char **argv ) {
     
     UE[CC_id]->UE_scan = UE_scan;
     UE[CC_id]->UE_scan_carrier = UE_scan_carrier;
+    UE[CC_id]->UE_fo_compensation = UE_fo_compensation;
     UE[CC_id]->mode    = mode;
     printf("UE[%d]->mode = %d\n",CC_id,mode);
     

targets/RT/USER/nr-uesoftmodem.h

@@ -55,6 +55,7 @@
 #define CONFIG_HLP_UENANTR       "set UE number of rx antennas\n"
 #define CONFIG_HLP_UENANTT       "set UE number of tx antennas\n"
 #define CONFIG_HLP_UESCAN        "set UE to scan around carrier\n"
+#define CONFIG_HLP_UEFO	         "set UE to enable estimation and compensation of frequency offset\n"
 #define CONFIG_HLP_DUMPFRAME     "dump UE received frame to rxsig_frame0.dat and exit\n"
 #define CONFIG_HLP_DLSHIFT       "dynamic shift for LLR compuation for TM3/4 (default 0)\n"
 #define CONFIG_HLP_UELOOP        "get softmodem (UE) to loop through memory instead of acquiring from HW\n"
@@ -135,6 +136,7 @@
 {"ue-nb-ant-rx",     	       CONFIG_HLP_UENANTR,    0,		u8ptr:&nb_antenna_rx,		    defuintval:1,   TYPE_UINT8,    0},     \
 {"ue-nb-ant-tx",     	       CONFIG_HLP_UENANTT,    0,		u8ptr:&nb_antenna_tx,		    defuintval:1,   TYPE_UINT8,    0},     \
 {"ue-scan-carrier",  	       CONFIG_HLP_UESCAN,     PARAMFLAG_BOOL,	iptr:&UE_scan_carrier,  	    defintval:0,    TYPE_INT,	   0},     \
+{"ue-fo-compensation", 	       CONFIG_HLP_UEFO,	      PARAMFLAG_BOOL,	iptr:&UE_fo_compensation,  	    defintval:0,    TYPE_INT,	   0},     \
 {"ue-max-power",     	       NULL,		      0,		iptr:&(tx_max_power[0]),	    defintval:90,   TYPE_INT,	   0},     \
 {"r"  ,                        CONFIG_HLP_PRB,        0,                iptr:&(frame_parms[0]->N_RB_DL),   defintval:25,   TYPE_UINT,    0},     \
 {"dlsch-demod-shift",     	 CONFIG_HLP_DLSHIFT,	0,		  iptr:(int32_t *)&dlsch_demod_shift,	defintval:0,			   TYPE_INT,	  0},			   \