diff --git a/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c b/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c index 1de1996126..a570f0a433 100644 --- a/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c +++ b/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c @@ -525,3 +525,21 @@ int nr_rx_pbch(PHY_VARS_NR_UE *ue, TracyCZoneEnd(ctx); return 0; } + +double nr_ue_pbch_freq_offset(const NR_DL_FRAME_PARMS *frame_parms, + int estimateSz, + const c16_t dl_ch_estimates[][estimateSz]) +{ + const int i_ssb = frame_parms->ssb_index; + const int symbol_offset = nr_get_ssb_start_symbol(frame_parms, i_ssb) % frame_parms->symbols_per_slot; + const c16_t *dl_ch_est_symb1 = &dl_ch_estimates[0][(symbol_offset + 1) * frame_parms->ofdm_symbol_size]; + const c16_t *dl_ch_est_symb3 = &dl_ch_estimates[0][(symbol_offset + 3) * frame_parms->ofdm_symbol_size]; + const int nb_re = 240; + const c32_t dot_prod_res = dot_product(dl_ch_est_symb1, dl_ch_est_symb3, nb_re, 8); + const double res_phase = atan2(dot_prod_res.i, dot_prod_res.r); + const int samples_per_symbol = frame_parms->ofdm_symbol_size + frame_parms->nb_prefix_samples; + const double t_ofdm = samples_per_symbol / (frame_parms->samples_per_subframe * 1000.0); // symbol duration in sec + const double freq_offset = res_phase / (2 * M_PI * (3 - 1) * t_ofdm); + + return freq_offset; +} diff --git a/openair1/PHY/NR_UE_TRANSPORT/nr_transport_proto_ue.h b/openair1/PHY/NR_UE_TRANSPORT/nr_transport_proto_ue.h index dc075753ef..bf68a69492 100644 --- a/openair1/PHY/NR_UE_TRANSPORT/nr_transport_proto_ue.h +++ b/openair1/PHY/NR_UE_TRANSPORT/nr_transport_proto_ue.h @@ -204,6 +204,10 @@ int nr_rx_pbch(PHY_VARS_NR_UE *ue, int rxdataFSize, const struct complex16 rxdataF[][rxdataFSize]); +double nr_ue_pbch_freq_offset(const NR_DL_FRAME_PARMS *frame_parms, + int estimateSz, + const c16_t dl_ch_estimates[][estimateSz]); + #ifndef modOrder #define modOrder(I_MCS,I_TBS) ((I_MCS-I_TBS)*2+2) // Find modulation order from I_TBS and I_MCS #endif diff --git a/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c b/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c index 37593bb9a7..54788de4d1 100644 --- a/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c +++ b/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c @@ -929,6 +929,11 @@ int pbch_pdcch_processing(PHY_VARS_NR_UE *ue, const UE_nr_rxtx_proc_t *proc, nr_ sampleShift = nr_adjust_synch_ue(fp, ue, gNB_id, fp->ofdm_symbol_size, dl_ch_estimates_time, frame_rx, nr_slot_rx, 16384); } + + if (/*ue->continuous_freq_correction &&*/ pbchSuccess == 0) { + double freq_offset = nr_ue_pbch_freq_offset(fp, estimateSz, dl_ch_estimates); + LOG_I(PHY,"detected frequency offset = %.3f Hz\n", freq_offset); + } } LOG_D(PHY, "Doing N0 measurements in %s\n", __FUNCTION__); nr_ue_rrc_measurements(ue, proc, rxdataF);