[develop] Added: Continous frequency offset estimation and compensation based on PBCH

    - PID controller implemented
    - post-compensation at UE for DL and pre-compensation at UE for UL

executables/nr-softmodem-common.h

@@ -122,6 +122,13 @@
 #define CONFIG_HLP_FDopplerVar   "Set Doppler variance, [fdoppler +/- fdopplerVar]\n"
 #define CONFIG_HLP_TDriftComp    "Execute continous timing drift compensation\n"
 #define CONFIG_HLP_FDopplerPrePost      "Set the pre/post compensation value for the Doppler shift at the gNB side\n"
+#define CONFIG_HLP_FP_ScalingFN  "Set the P scaling factor (numerator) of the PID controller for the Doppler compensation at UE side"
+#define CONFIG_HLP_FP_ScalingFD  "Set the P scaling factor (denominator) of the PID controller for the Doppler compensation at UE side"
+#define CONFIG_HLP_FI_ScalingFN  "Set the I scaling factor (numerator) of the PID controller for the Doppler compensation at UE side"
+#define CONFIG_HLP_FI_ScalingFD  "Set the I scaling factor (denominator) of the PID controller for the Doppler compensation at UE side"
+#define CONFIG_HLP_FD_ScalingFN  "Set the D scaling factor (numerator) of the PID controller for the Doppler compensation at UE side"
+#define CONFIG_HLP_FD_ScalingFD  "Set the D scaling factor (denominator) of the PID controller for the Doppler compensation at UE side"
+
 /*--------------------------------------------------------------------------------------------------------------------------------*/
 /*                                            command line parameters for LOG utility                                             */
 /*   optname         helpstr          paramflags          XXXptr                     defXXXval            type           numelt   */
@@ -178,5 +185,11 @@ extern int32_t fdopplerRate; //Doppler rate in Hz/s
 extern uint32_t fdopplerVar; //Doppler variance, [fdoppler +/- fdopplerVar]
 extern int tdriftComp;         // flag to activate/deactivate continous timing drift compensation
 extern int32_t fdopplerPrePost; //pre/post compensation of the Doppler shift at the gNB side
+extern uint16_t P_ScalingFN; //P scaling factor (numerator) of the PID controller for the Doppler compensation at UE side
+extern uint16_t P_ScalingFD; //P scaling factor (denominator) of the PID controller for the Doppler compensation at UE side
+extern uint16_t I_ScalingFN; //I scaling factor (numerator) of the PID controller for the Doppler compensation at UE side
+extern uint16_t I_ScalingFD; //I scaling factor (denominator) of the PID controller for the Doppler compensation at UE side
+extern uint16_t D_ScalingFN; //D scaling factor (numerator) of the PID controller for the Doppler compensation at UE side
+extern uint16_t D_ScalingFD; //D scaling factor (denominator) of the PID controller for the Doppler compensation at UE side
 
 #endif

executables/nr-ue.c

@@ -770,6 +770,7 @@ void *UE_thread(void *arg)
   int absolute_slot=0, decoded_frame_rx=INT_MAX, trashed_frames=0;
   initNotifiedFIFO(&UE->phy_config_ind);
 
+  InitSinLUT();
   int num_ind_fifo = nb_slot_frame;
   for(int i=0; i < num_ind_fifo; i++) {
     UE->tx_wait_for_dlsch[num_ind_fifo] = 0;

executables/nr-uesoftmodem.c

@@ -434,6 +434,12 @@ uint32_t fdopplerVar; //Doppler variance, [fdoppler +/- fdopplerVar]
 int fdopplerComp = 1; // flag to activate continous frequency offset compensation (=0 deacticated, =1 activated (default case))
 int tdriftComp = 1;
 int32_t fdopplerPrePost = 0; //pre/post compensation of the Doppler shift at the gNB side. Dummy definition at UE to avoid linking error.
+uint16_t P_ScalingFN = 1; //P scaling factor (numerator) of the PID controller for the Doppler compensation at UE side
+uint16_t P_ScalingFD = 3; //P scaling factor (denominator) of the PID controller for the Doppler compensation at UE side
+uint16_t I_ScalingFN = 1; //I scaling factor (numerator) of the PID controller for the Doppler compensation at UE side
+uint16_t I_ScalingFD = 2; //I scaling factor (denominator) of the PID controller for the Doppler compensation at UE side
+uint16_t D_ScalingFN = 0; //D scaling factor (numerator) of the PID controller for the Doppler compensation at UE side
+uint16_t D_ScalingFD = 1; //D scaling factor (denominator) of the PID controller for the Doppler compensation at UE side
 
 
 int main( int argc, char **argv ) {

executables/nr-uesoftmodem.h

@@ -71,7 +71,13 @@
   {"TD" ,                          CONFIG_HLP_TDRIFT,      0,                   .iptr=&RFsim_DriftPerFrame,                  .defintval=0,      TYPE_INT,      0}, \
   {"DSR" ,                         CONFIG_HLP_FDopplerRate,0,                   .iptr=&fdopplerRate,                         .defintval=0,      TYPE_INT32,    0}, \
   {"DSV" ,                         CONFIG_HLP_FDopplerVar, 0,                   .uptr=&fdopplerVar,                          .defintval=0,      TYPE_UINT32,   0}, \
-  {"TC" ,                          CONFIG_HLP_TDriftComp,    0,                 .iptr=&tdriftComp,                           .defintval=1,      TYPE_INT,    0}, \
+  {"TC" ,                          CONFIG_HLP_TDriftComp,    0,                 .iptr=&tdriftComp,                           .defintval=1,      TYPE_INT,      0}, \
+  {"DCPN" ,                        CONFIG_HLP_FP_ScalingFN,0,                   .u16ptr=&P_ScalingFN,                        .defintval=1,      TYPE_UINT16,   0}, \
+  {"DCPD" ,                        CONFIG_HLP_FP_ScalingFD,0,                   .u16ptr=&P_ScalingFD,                        .defintval=3,      TYPE_UINT16,   0}, \
+  {"DCIN" ,                        CONFIG_HLP_FI_ScalingFN,0,                   .u16ptr=&I_ScalingFN,                        .defintval=1,      TYPE_UINT16,   0}, \
+  {"DCID" ,                        CONFIG_HLP_FI_ScalingFD,0,                   .u16ptr=&I_ScalingFD,                        .defintval=2,      TYPE_UINT16,   0}, \
+  {"DCDN" ,                        CONFIG_HLP_FD_ScalingFN,0,                   .u16ptr=&D_ScalingFN,                        .defintval=0,      TYPE_UINT16,   0}, \
+  {"DCDD" ,                        CONFIG_HLP_FD_ScalingFD,0,                   .u16ptr=&D_ScalingFD,                        .defintval=1,      TYPE_UINT16,   0}  \
 }
 // clang-format on
 

openair1/PHY/INIT/nr_init_ue.c

@@ -381,6 +381,9 @@ int init_nr_ue_signal(PHY_VARS_NR_UE *ue, int nb_connected_gNB)
   init_symbol_rotation(fp);
   init_timeshift_rotation(fp);
 
+  // set the initial frequency offset to 0
+  ue->DopplerEst = 0;
+  
   return 0;
 }
 

openair1/PHY/MODULATION/slot_fep_nr.c

@@ -35,6 +35,8 @@
 #define LOG_I(A,B...) printf(A)
 #endif*/
 
+extern int fdopplerComp; 
+
 int nr_slot_fep(PHY_VARS_NR_UE *ue,
                 UE_nr_rxtx_proc_t *proc,
                 unsigned char symbol,
@@ -67,6 +69,15 @@ int nr_slot_fep(PHY_VARS_NR_UE *ue,
     rx_offset += (idx_symb%(0x7<<frame_parms->numerology_index)) ? nb_prefix_samples : nb_prefix_samples0;
   rx_offset += frame_parms->ofdm_symbol_size * symbol;
 
+  unsigned int curr_nb_prefix = 0;
+  uint32_t SampIdxDopplerUERx = 0;
+
+  if (fdopplerComp == 1)
+  {
+    curr_nb_prefix = (abs_symbol%(0x7<<frame_parms->numerology_index)) ? nb_prefix_samples : nb_prefix_samples0;
+    SampIdxDopplerUERx = rx_offset - curr_nb_prefix; //sample index in the calculation of the Doppler shift compensation for a slot
+  }
+
   // use OFDM symbol from within 1/8th of the CP to avoid ISI
   rx_offset -= (nb_prefix_samples / frame_parms->ofdm_offset_divisor);
 
@@ -77,6 +88,15 @@ int nr_slot_fep(PHY_VARS_NR_UE *ue,
   //#endif
 
   for (unsigned char aa=0; aa<frame_parms->nb_antennas_rx; aa++) {
+
+    if (fdopplerComp == 1)
+    {
+      unsigned int nsamps = frame_parms->ofdm_symbol_size + curr_nb_prefix; 
+      int16_t *rxdataDopp_ptr = (int16_t *)&common_vars->rxdata[aa][SampIdxDopplerUERx];
+      int DopplerToComp = -(ue->DopplerEst);
+      nr_apply_Doppler( rxdataDopp_ptr, nsamps, DopplerToComp , &SampIdxDopplerUERx, frame_parms);
+    }
+    
     int16_t *rxdata_ptr = (int16_t *)&common_vars->rxdata[aa][rx_offset];
 
     // if input to dft is not 256-bit aligned
@@ -221,7 +241,6 @@ int nr_slot_fep_init_sync(PHY_VARS_NR_UE *ue,
 }
 
 extern int fdopplerPrePost; //Doppler frequency shift
-extern int fdopplerComp; 
 int nr_slot_fep_ul(NR_DL_FRAME_PARMS *frame_parms,
                    int32_t *rxdata,
                    int32_t *rxdataF,

openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c

@@ -38,6 +38,8 @@
 #include "openair1/SCHED_NR_UE/defs.h"
 #include <openair1/PHY/NR_UE_TRANSPORT/nr_transport_proto_ue.h>
 #include <openair1/PHY/TOOLS/phy_scope_interface.h>
+#include <complex.h>
+#include "executables/nr-uesoftmodem.h"
 
 //#define DEBUG_PBCH
 //#define DEBUG_PBCH_ENCODING
@@ -46,6 +48,7 @@
 
 #define PBCH_A 24
 #define PBCH_MAX_RE_PER_SYMBOL (20*12)
+#define PBCH_DMRS_PER_SYMBOL (20*3)           // 20 PRBs, and 3 DMRS in each of them --> in total 60 channel estimates per symbol (symbol 1 and 3)
 #define PBCH_MAX_RE (PBCH_MAX_RE_PER_SYMBOL*4)
 #define print_shorts(s,x) printf("%s : %d,%d,%d,%d,%d,%d,%d,%d\n",s,((int16_t*)x)[0],((int16_t*)x)[1],((int16_t*)x)[2],((int16_t*)x)[3],((int16_t*)x)[4],((int16_t*)x)[5],((int16_t*)x)[6],((int16_t*)x)[7])
 
@@ -55,11 +58,26 @@ static uint16_t nr_pbch_extract(uint32_t rxdataF_sz,
                                 struct complex16 dl_ch_estimates[][estimateSz],
                                 struct complex16 rxdataF_ext[][PBCH_MAX_RE_PER_SYMBOL],
                                 struct complex16 dl_ch_estimates_ext[][PBCH_MAX_RE_PER_SYMBOL],
+                                struct complex16 dl_ch_estimates_dmrs[][PBCH_DMRS_PER_SYMBOL],
                                 uint32_t symbol,
                                 uint32_t s_offset,
                                 NR_DL_FRAME_PARMS *frame_parms) {
+  // check if PBCH crosses the DC, and find the affected RE
+  int reCrossingZero = (int)((12*frame_parms->N_RB_DL)/2) + frame_parms->first_carrier_offset;
+  uint16_t start_subcarrier = frame_parms->first_carrier_offset + frame_parms->ssb_start_subcarrier;
+  uint16_t idxInPBCH = 0;   // index of the RE in the PBCH crossing the DC
+
+  if ((start_subcarrier < reCrossingZero) && (start_subcarrier+20*12 >= reCrossingZero)) {
+    idxInPBCH = (uint16_t)reCrossingZero - start_subcarrier;
+  }
+
+#ifdef DEBUG_PBCH
+  LOG_I(PHY, "*** start_subcarrier: %u, ofdm_symbol_size: %u, first_carrier_offset: %u, ssb_start_subcarrier: %u, idxInPBCH: %u, reCrossingZero: %i\n",
+              start_subcarrier, frame_parms->ofdm_symbol_size, frame_parms->first_carrier_offset, frame_parms->ssb_start_subcarrier, idxInPBCH, reCrossingZero);
+#endif
+
   uint16_t rb;
-  uint8_t i,j,aarx;
+  uint8_t i,j,aarx,k;
   int nushiftmod4 = frame_parms->nushift;
   AssertFatal(symbol>=1 && symbol<5,
               "symbol %d illegal for PBCH extraction\n",
@@ -137,9 +155,12 @@ static uint16_t nr_pbch_extract(uint32_t rxdataF_sz,
 
     //printf("dl_ch0 addr %p\n",dl_ch0);
     struct complex16 *dl_ch0_ext = dl_ch_estimates_ext[aarx];
+    struct complex16 *dl_ch0_dmrs = dl_ch_estimates_dmrs[aarx];
+    int reCounter = 0;
 
     for (rb=0; rb<20; rb++) {
       j=0;
+      k=0;
 
       if (symbol==1 || symbol==3) {
         for (i=0; i<12; i++) {
@@ -156,10 +177,21 @@ static uint16_t nr_pbch_extract(uint32_t rxdataF_sz,
 #endif
             j++;
           }
+          else
+          {
+            if (reCounter != idxInPBCH)
+            {
+              dl_ch0_dmrs[k]=dl_ch0[i];
+              k++;
+            }
+          }
+          reCounter++;
         }
 
         dl_ch0+=12;
         dl_ch0_ext+=9;
+        if (reCounter != idxInPBCH)
+          dl_ch0_dmrs+=3;
       } else {
         if ((rb < 4) || (rb >15)) {
           for (i=0; i<12; i++) {
@@ -427,17 +459,28 @@ int nr_rx_pbch(PHY_VARS_NR_UE *ue,
   // symbol refers to symbol within SSB. symbol_offset is the offset of the SSB wrt start of slot
   double log2_maxh = 0;
 
+  /* ***************************************************
+  here implement the doppler estimation based on PBCH DMRS
+  ***************************************************** */
+  int16_t DMRS_idx_current = 3;
+  int16_t DMRS_idx_last = 1;
+
+  __attribute__ ((aligned(32))) struct complex16 dl_ch_estimates_dmrs_symb1[frame_parms->nb_antennas_rx][PBCH_DMRS_PER_SYMBOL];
+
   for (symbol=1; symbol<4; symbol++) {
     const uint16_t nb_re=symbol == 2 ? 72 : 180;
     __attribute__ ((aligned(32))) struct complex16 rxdataF_ext[frame_parms->nb_antennas_rx][PBCH_MAX_RE_PER_SYMBOL];
     __attribute__ ((aligned(32))) struct complex16 dl_ch_estimates_ext[frame_parms->nb_antennas_rx][PBCH_MAX_RE_PER_SYMBOL];
     memset(dl_ch_estimates_ext,0, sizeof  dl_ch_estimates_ext);
+    __attribute__ ((aligned(32))) struct complex16 dl_ch_estimates_dmrs[frame_parms->nb_antennas_rx][PBCH_DMRS_PER_SYMBOL];
+    memset(dl_ch_estimates_dmrs,0, sizeof  dl_ch_estimates_dmrs);
     nr_pbch_extract(ue->frame_parms.samples_per_slot_wCP,
                     rxdataF,
                     estimateSz,
                     dl_ch_estimates,
                     rxdataF_ext,
                     dl_ch_estimates_ext,
+                    dl_ch_estimates_dmrs,
                     symbol,
                     symbol_offset,
                     frame_parms);
@@ -451,6 +494,50 @@ int nr_rx_pbch(PHY_VARS_NR_UE *ue,
                                     frame_parms,
                                     nb_re);
       log2_maxh = 3+(log2_approx(max_h)/2);
+      memcpy(&dl_ch_estimates_dmrs_symb1[0], &dl_ch_estimates_dmrs[0], sizeof dl_ch_estimates_dmrs[0]);
+    }
+
+    if (symbol == 3){
+      //For the PI controller
+      static int64_t Doppler_I_Ctrl = 0; //Integral controller for Doppler
+      static int64_t DopplerErrLast = (int64_t)1<<60; //Doppler from last estimation
+      int64_t DopplerErr = 0;
+
+      int16_t *dlChEstSymb1 = (int16_t*)&dl_ch_estimates_dmrs_symb1[0];
+      int16_t *dlChEstSymb3 = (int16_t*)&dl_ch_estimates_dmrs[0];
+
+      int nbRE = PBCH_DMRS_PER_SYMBOL;
+      int channelLevel = nr_pbch_channel_level(dl_ch_estimates_ext, frame_parms, nbRE);
+      uint8_t channelLevelLog = log2_approx(channelLevel);
+      // the normalization factor has been derived based on multiple observations for different channel level values.
+      uint8_t outputShift = (uint8_t)((float)channelLevelLog*0.8 - 2.75);
+      nbRE = 56;   // multiple of 8, as implemnted at the dot product function
+      const c32_t Dot_Prod_Res = dot_product( dlChEstSymb1, dlChEstSymb3, nbRE, outputShift);
+      //int32_t Dot_Prod_Res = dot_product( dlChEstSymb1, dlChEstSymb3, nbRE, outputShift);
+      //struct complex16 Res_cpx;
+      //Res_cpx.r = ((struct complex16*) &Dot_Prod_Res)->r;
+      //Res_cpx.i = ((struct complex16*) &Dot_Prod_Res)->i;
+      //double Res_phase = atan2( (double)Res_cpx.i, (double)Res_cpx.r);
+      double Res_phase = atan2( (double)Dot_Prod_Res.i, (double)Dot_Prod_Res.r);
+      double slotDuration = 0.01/((double)ue->frame_parms.slots_per_frame);       // slot duration in sec
+      double TOfdm = slotDuration / ((double)ue->frame_parms.symbols_per_slot);   // symbol duration in sec
+      double DopplerEst = Res_phase/ (2*M_PI*(DMRS_idx_current-DMRS_idx_last)*TOfdm);
+
+      // PI Controller
+      DopplerErr = (int64_t)DopplerEst;
+      if ( DopplerErrLast == (int64_t)1<<60 ) //Initialization of DopplerErrLast
+        DopplerErrLast = DopplerErr;
+      Doppler_I_Ctrl += DopplerErr;
+      ue->DopplerEst = (int32_t)(DopplerErr*P_ScalingFN/P_ScalingFD + Doppler_I_Ctrl*I_ScalingFN/I_ScalingFD +
+        (DopplerErr-DopplerErrLast)*D_ScalingFN/D_ScalingFD); //PID controller
+      DopplerErrLast = DopplerErr;
+
+#ifdef DEBUG_PBCH
+      double rx_gain = openair0_cfg[0].rx_gain[0];
+      double rx_gain_offset = openair0_cfg[0].rx_gain_offset[0];
+      LOG_I(PHY, "**** DopplerEst: %f, ue->DopplerEst: %d, chLevel: %i, chLevelLog: %u, outShift: %u, re: %i, im: %i, phase: %f, rxG: %f, rxGOff: %f\n",
+            DopplerEst, ue->DopplerEst, channelLevel, channelLevelLog, outputShift, Res_cpx.r, Res_cpx.i, Res_phase, rx_gain, rx_gain_offset);
+#endif
     }
 
 #ifdef DEBUG_PBCH

openair1/PHY/NR_UE_TRANSPORT/nr_ulsch_ue.c

@@ -595,6 +595,7 @@ uint8_t nr_ue_pusch_common_procedures(PHY_VARS_NR_UE *UE,
                                       c16_t **txdataF)
 {
   const int tx_offset = frame_parms->get_samples_slot_timestamp(slot, frame_parms, 0);
+  uint32_t SampIdxDopplerUETx; //sample index for applying the Doppler shift at UE side for UL
 
   c16_t **txdata = UE->common_vars.txData;
   for(int ap = 0; ap < n_antenna_ports; ap++) {
@@ -624,6 +625,18 @@ uint8_t nr_ue_pusch_common_procedures(PHY_VARS_NR_UE *UE,
     }
   }
 
+  SampIdxDopplerUETx = frame_parms->get_samples_slot_timestamp(slot,frame_parms,0);
+  int writeBlockSize = frame_parms->get_samples_per_slot(slot,frame_parms);
+  extern int fdopplerComp;
+  if (fdopplerComp == 1){
+    // First scale the offset computed on DL before being applied to UL
+    uint64_t dl_carrier, ul_carrier;
+    nr_get_carrier_frequencies(UE, &dl_carrier, &ul_carrier);
+    double freq_scale  = (double)ul_carrier / dl_carrier;
+    int DopplerToComp = -(UE->DopplerEst * freq_scale);
+    nr_apply_Doppler( &txdata[0][tx_offset], writeBlockSize, DopplerToComp, &SampIdxDopplerUETx, frame_parms);
+  }
+
   ///////////
   ////////////////////////////////////////////////////
   return 0;

openair1/PHY/defs_nr_UE.h

@@ -529,6 +529,9 @@ typedef struct {
   int              rx_offset_slot; /// Indicating time (in slots) after rx_offset calculation
   int              rx_offset_comp; /// Already compensated rx_offset
 
+  // Estimated Doppler frequency shift from the DMRS
+  int32_t DopplerEst;
+
   /// Timing Advance updates variables
   /// Timing advance update computed from the TA command signalled from gNB
   int timing_advance;