some conditions on frequency offset tracking added

openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c

@@ -414,6 +414,8 @@ unsigned char sign(int8_t x) {
 
 const uint8_t pbch_deinterleaving_pattern[32] = {28, 0, 31, 30, 7,  29, 25, 27, 5,  8,  24, 9,  10, 11, 12, 13,
                                                  1,  4, 3,  14, 15, 16, 17, 2,  26, 18, 19, 20, 21, 22, 6,  23};
+
+static int DopplerEstTot = 0;
 int nr_rx_pbch(PHY_VARS_NR_UE *ue,
                UE_nr_rxtx_proc_t *proc,
                int estimateSz,
@@ -517,26 +519,29 @@ int nr_rx_pbch(PHY_VARS_NR_UE *ue,
       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
+      if ((ue->is_synchronized) && (DopplerEst > 35 || DopplerEst < -35 ))
+      { 
+        // 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
+        ue->DopplerEst = (int32_t)( (DopplerErr * FO_PScaling) + (Doppler_I_Ctrl * FO_IScaling) ); //PI controller
         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
-      ue->DopplerEst = (int32_t)( (DopplerErr * FO_PScaling) + (Doppler_I_Ctrl * FO_IScaling) ); //PI controller
-      DopplerErrLast = DopplerErr;
-	  ue->DopplerEstTot += (float)ue->DopplerEst;
+        ue->DopplerEstTot += (float)ue->DopplerEst;
+        DopplerEstTot += ue->DopplerEst;
+      } 
 	  
-	  //extern int commonDoppler;
-	  //LOG_A(PHY, "commonDoppler: %d, DopplerEst: %f, ue->DopplerEst: %d, ue->DopplerEstTot: %f\n", commonDoppler, DopplerEst, ue->DopplerEst, ue->DopplerEstTot);
-
 #ifdef DEBUG_PBCH
       double rx_gain = openair0_cfg[0].rx_gain[0];
       double DopplerEstMax = M_PI/ (2*M_PI*(DMRS_idx_current-DMRS_idx_last)*TOfdm);
       LOG_I(PHY, "**** DopplerEst: %f, ue->DopplerEst: %d, chLevel: %i, chLevelLog: %u, outShift: %u, re: %i, im: %i, phase: %f, rxG: %f\n",
             DopplerEst, ue->DopplerEst, channelLevel, channelLevelLog, outputShift, Dot_Prod_Res.r, Dot_Prod_Res.i, Res_phase, rx_gain);
       LOG_I(PHY, "DopplerEstMax: %f, PScaling: %f, IScaling: %f\n", DopplerEstMax, PScaling, IScaling);
+	    extern int commonDoppler;
+	    LOG_I(PHY, "commonDoppler: %d, DopplerEst: %f, ue->DopplerEst: %d, DopplerEstTot: %d\n", commonDoppler, DopplerEst, ue->DopplerEst, DopplerEstTot);
 #endif
     }