From 6d355ab61e3d7cb90c151f2785ead11b5156027a Mon Sep 17 00:00:00 2001 From: Xavier Arteaga Date: Thu, 26 Mar 2020 19:52:08 +0100 Subject: [PATCH] SRSUE: Fix HO initial CFO --- lib/include/srslte/interfaces/ue_interfaces.h | 7 ++-- lib/include/srslte/phy/sync/sync.h | 2 +- lib/include/srslte/phy/ue/ue_sync.h | 2 +- lib/src/phy/sync/sync.c | 6 ++-- lib/src/phy/ue/ue_cell_search.c | 2 +- lib/src/phy/ue/ue_sync.c | 12 +++---- srsue/hdr/phy/phy.h | 2 +- srsue/hdr/phy/phy_common.h | 1 + srsue/hdr/phy/sync.h | 4 +-- srsue/hdr/stack/rrc/rrc.h | 13 ++++++-- srsue/src/phy/phy.cc | 4 +-- srsue/src/phy/scell/intra_measure.cc | 1 + srsue/src/phy/scell/scell_recv.cc | 2 +- srsue/src/phy/sf_worker.cc | 1 + srsue/src/phy/sync.cc | 32 ++++++++++--------- srsue/src/stack/rrc/rrc.cc | 18 ++++++----- 16 files changed, 63 insertions(+), 46 deletions(-) diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h index efa453ccf..0451d7a6a 100644 --- a/lib/include/srslte/interfaces/ue_interfaces.h +++ b/lib/include/srslte/interfaces/ue_interfaces.h @@ -127,6 +127,7 @@ public: typedef struct { float rsrp; float rsrq; + float cfo_hz; uint32_t earfcn; uint32_t pci; } phy_meas_t; @@ -578,9 +579,9 @@ public: } phy_cell_t; /* Cell search and selection procedures */ - virtual cell_search_ret_t cell_search(phy_cell_t* cell) = 0; - virtual bool cell_select(const phy_cell_t* cell = NULL) = 0; - virtual bool cell_is_camping() = 0; + virtual cell_search_ret_t cell_search(phy_cell_t* cell) = 0; + virtual bool cell_select(const phy_cell_t* cell = nullptr, float cfo_hz = 0.0f) = 0; + virtual bool cell_is_camping() = 0; virtual void reset() = 0; diff --git a/lib/include/srslte/phy/sync/sync.h b/lib/include/srslte/phy/sync/sync.h index aaaceeecb..b6c09841f 100644 --- a/lib/include/srslte/phy/sync/sync.h +++ b/lib/include/srslte/phy/sync/sync.h @@ -184,7 +184,7 @@ SRSLTE_API void srslte_sync_set_sss_eq_enable(srslte_sync_t* q, bool enable); SRSLTE_API float srslte_sync_get_cfo(srslte_sync_t* q); /* Resets internal CFO state */ -SRSLTE_API void srslte_sync_cfo_reset(srslte_sync_t* q); +SRSLTE_API void srslte_sync_cfo_reset(srslte_sync_t* q, float cfo_Hz); /* Copies CFO internal state from another object to avoid long transients */ SRSLTE_API void srslte_sync_copy_cfo(srslte_sync_t* q, srslte_sync_t* src_obj); diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h index 51b71732e..227a89528 100644 --- a/lib/include/srslte/phy/ue/ue_sync.h +++ b/lib/include/srslte/phy/ue/ue_sync.h @@ -203,7 +203,7 @@ SRSLTE_API void srslte_ue_sync_file_wrap(srslte_ue_sync_t *q, SRSLTE_API int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell); -SRSLTE_API void srslte_ue_sync_cfo_reset(srslte_ue_sync_t *q); +SRSLTE_API void srslte_ue_sync_cfo_reset(srslte_ue_sync_t* q, float init_cfo_hz); SRSLTE_API void srslte_ue_sync_reset(srslte_ue_sync_t *q); diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c index 29967bd2e..3f549ab2c 100644 --- a/lib/src/phy/sync/sync.c +++ b/lib/src/phy/sync/sync.c @@ -80,7 +80,7 @@ int srslte_sync_init_decim(srslte_sync_t* q, uint32_t frame_size, uint32_t max_o q->max_offset = max_offset; q->max_frame_size = frame_size; - srslte_sync_cfo_reset(q); + srslte_sync_cfo_reset(q, 0.0f); if (srslte_cfo_init(&q->cfo_corr_frame, q->frame_size)) { ERROR("Error initiating CFO\n"); @@ -347,9 +347,9 @@ float srslte_sync_get_cfo(srslte_sync_t* q) return q->cfo_cp_mean + q->cfo_pss_mean + q->cfo_i_value; } -void srslte_sync_cfo_reset(srslte_sync_t* q) +void srslte_sync_cfo_reset(srslte_sync_t* q, float init_cfo_hz) { - q->cfo_cp_mean = 0; + q->cfo_cp_mean = init_cfo_hz / 15e3f; q->cfo_cp_is_set = false; q->cfo_pss_mean = 0; q->cfo_pss_is_set = false; diff --git a/lib/src/phy/ue/ue_cell_search.c b/lib/src/phy/ue/ue_cell_search.c index 28b2eadd5..17f0eb45e 100644 --- a/lib/src/phy/ue/ue_cell_search.c +++ b/lib/src/phy/ue/ue_cell_search.c @@ -303,7 +303,7 @@ int srslte_ue_cellsearch_scan_N_id_2(srslte_ue_cellsearch_t* q, srslte_ue_sync_set_N_id_2(&q->ue_sync, N_id_2); srslte_ue_sync_reset(&q->ue_sync); - srslte_ue_sync_cfo_reset(&q->ue_sync); + srslte_ue_sync_cfo_reset(&q->ue_sync, 0.0f); srslte_ue_sync_set_nof_find_frames(&q->ue_sync, q->max_frames); do { diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c index 7ff5cfd2c..b6683ff9f 100644 --- a/lib/src/phy/ue/ue_sync.c +++ b/lib/src/phy/ue/ue_sync.c @@ -102,7 +102,7 @@ int srslte_ue_sync_init_file_multi(srslte_ue_sync_t* q, free(file_offset_buffer); } - srslte_ue_sync_cfo_reset(q); + srslte_ue_sync_cfo_reset(q, 0.0f); srslte_ue_sync_reset(q); ret = SRSLTE_SUCCESS; @@ -114,12 +114,12 @@ clean_exit: return ret; } -void srslte_ue_sync_cfo_reset(srslte_ue_sync_t* q) +void srslte_ue_sync_cfo_reset(srslte_ue_sync_t* q, float init_cfo_hz) { q->cfo_is_copied = false; - q->cfo_current_value = 0; - srslte_sync_cfo_reset(&q->strack); - srslte_sync_cfo_reset(&q->sfind); + q->cfo_current_value = init_cfo_hz / 15e3f; + srslte_sync_cfo_reset(&q->strack, init_cfo_hz); + srslte_sync_cfo_reset(&q->sfind, init_cfo_hz); } void srslte_ue_sync_reset(srslte_ue_sync_t* q) @@ -609,7 +609,7 @@ static int find_peak_ok(srslte_ue_sync_t* q, cf_t* input_buffer[SRSLTE_MAX_CHANN if (!q->cfo_is_copied) { q->cfo_current_value = srslte_sync_get_cfo(&q->sfind); } - srslte_sync_cfo_reset(&q->strack); + srslte_sync_cfo_reset(&q->strack, 0.0f); } if (q->cell.id < 1000) { diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h index 8d3595cc0..28219e09f 100644 --- a/srsue/hdr/phy/phy.h +++ b/srsue/hdr/phy/phy.h @@ -69,7 +69,7 @@ public: /********** RRC INTERFACE ********************/ void reset() final; cell_search_ret_t cell_search(phy_cell_t* cell) final; - bool cell_select(const phy_cell_t* cell) final; + bool cell_select(const phy_cell_t* cell, float cfo) final; void set_cells_to_meas(uint32_t earfcn, const std::set& pci) final; void meas_stop() final; diff --git a/srsue/hdr/phy/phy_common.h b/srsue/hdr/phy/phy_common.h index 32310b3bf..a3d511402 100644 --- a/srsue/hdr/phy/phy_common.h +++ b/srsue/hdr/phy/phy_common.h @@ -65,6 +65,7 @@ public: float avg_rsrp_dbm[SRSLTE_MAX_CARRIERS] = {}; float avg_rsrq_db[SRSLTE_MAX_CARRIERS] = {}; float avg_rssi_dbm[SRSLTE_MAX_CARRIERS] = {}; + float avg_cfo_hz[SRSLTE_MAX_CARRIERS] = {}; float rx_gain_offset = 0.0f; float avg_snr_db_cqi[SRSLTE_MAX_CARRIERS] = {}; float avg_noise[SRSLTE_MAX_CARRIERS] = {}; diff --git a/srsue/hdr/phy/sync.h b/srsue/hdr/phy/sync.h index 99a3ae8ee..088bf398e 100644 --- a/srsue/hdr/phy/sync.h +++ b/srsue/hdr/phy/sync.h @@ -65,7 +65,7 @@ public: // RRC interface for controling the SYNC state phy_interface_rrc_lte::cell_search_ret_t cell_search(phy_interface_rrc_lte::phy_cell_t* cell); - bool cell_select(const phy_interface_rrc_lte::phy_cell_t* cell); + bool cell_select(const phy_interface_rrc_lte::phy_cell_t* cell, float cfo); bool cell_is_camping(); // RRC interface for controlling the neighbour cell measurement @@ -164,7 +164,7 @@ private: void set_sampling_rate(); bool set_frequency(); - bool set_cell(); + bool set_cell(float cfo); bool running = false; bool is_overflow = false; diff --git a/srsue/hdr/stack/rrc/rrc.h b/srsue/hdr/stack/rrc/rrc.h index 5874c63e6..4707b6e80 100644 --- a/srsue/hdr/stack/rrc/rrc.h +++ b/srsue/hdr/stack/rrc/rrc.h @@ -148,9 +148,16 @@ public: rsrq = rsrq_; } } + void set_cfo(float cfo_Hz_) + { + if (not std::isnan(cfo_Hz_) && not std::isinf(cfo_Hz_)) { + cfo_hz = cfo_Hz_; + } + } float get_rsrp() { return rsrp; } float get_rsrq() { return rsrq; } + float get_cfo_hz() { return cfo_hz; } void set_sib1(asn1::rrc::sib_type1_s* sib1_); void set_sib2(asn1::rrc::sib_type2_s* sib2_); @@ -235,11 +242,12 @@ public: char buf[256]; snprintf(buf, 256, - "{cell_id: 0x%x, pci: %d, dl_earfcn: %d, rsrp=%+.1f}", + "{cell_id: 0x%x, pci: %d, dl_earfcn: %d, rsrp=%+.1f, cfo=%+.1f}", get_cell_id(), get_pci(), get_earfcn(), - get_rsrp()); + get_rsrp(), + get_cfo_hz()); return std::string{buf}; } @@ -256,6 +264,7 @@ public: private: float rsrp = NAN; float rsrq = NAN; + float cfo_hz = 0.0f; struct timeval last_update = {}; diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc index 75e8b83b4..801fd9384 100644 --- a/srsue/src/phy/phy.cc +++ b/srsue/src/phy/phy.cc @@ -314,9 +314,9 @@ void phy::meas_stop() sfsync.meas_stop(); } -bool phy::cell_select(const phy_cell_t* cell) +bool phy::cell_select(const phy_cell_t* cell, float cfo) { - return sfsync.cell_select(cell); + return sfsync.cell_select(cell, cfo); } phy_interface_rrc_lte::cell_search_ret_t phy::cell_search(phy_cell_t* cell) diff --git a/srsue/src/phy/scell/intra_measure.cc b/srsue/src/phy/scell/intra_measure.cc index 5d987f842..505844aa0 100644 --- a/srsue/src/phy/scell/intra_measure.cc +++ b/srsue/src/phy/scell/intra_measure.cc @@ -192,6 +192,7 @@ void intra_measure::measure_proc() m.earfcn = current_earfcn; m.rsrp = refsignal_dl_sync.rsrp_dBfs - rx_gain_offset_db; m.rsrq = refsignal_dl_sync.rsrq_dB; + m.cfo_hz = refsignal_dl_sync.cfo_Hz; neighbour_cells.push_back(m); Info("INTRA: Found neighbour cell: EARFCN=%d, PCI=%03d, RSRP=%5.1f dBm, RSRQ=%5.1f, peak_idx=%5d, " diff --git a/srsue/src/phy/scell/scell_recv.cc b/srsue/src/phy/scell/scell_recv.cc index 8001443e0..b879ce488 100644 --- a/srsue/src/phy/scell/scell_recv.cc +++ b/srsue/src/phy/scell/scell_recv.cc @@ -106,7 +106,7 @@ scell_recv::find_cells(const cf_t* input_buffer, const srslte_cell_t serving_cel srslte_sync_find_ret_t sync_res; srslte_sync_reset(&sync_find); - srslte_sync_cfo_reset(&sync_find); + srslte_sync_cfo_reset(&sync_find, 0.0f); sync_res = SRSLTE_SYNC_NOFOUND; bool sss_detected = false; diff --git a/srsue/src/phy/sf_worker.cc b/srsue/src/phy/sf_worker.cc index dae3f1348..d50479464 100644 --- a/srsue/src/phy/sf_worker.cc +++ b/srsue/src/phy/sf_worker.cc @@ -332,6 +332,7 @@ void sf_worker::update_measurements() rrc_interface_phy_lte::phy_meas_t meas = {}; meas.rsrp = phy->avg_rsrp_dbm[cc_idx]; meas.rsrq = phy->avg_rsrq_db[cc_idx]; + meas.cfo_hz = phy->avg_cfo_hz[cc_idx]; // Save EARFCN and PCI for secondary cells, primary cell has earfcn=0 if (cc_idx > 0) { meas.earfcn = phy->scell_cfg[cc_idx].earfcn; diff --git a/srsue/src/phy/sync.cc b/srsue/src/phy/sync.cc index 2d5e79af0..23947d23f 100644 --- a/srsue/src/phy/sync.cc +++ b/srsue/src/phy/sync.cc @@ -252,7 +252,7 @@ phy_interface_rrc_lte::cell_search_ret_t sync::cell_search(phy_interface_rrc_lte /* Cell select synchronizes to a new cell (e.g. during HO or during cell reselection on IDLE) or * re-synchronizes with the current cell if cell argument is NULL */ -bool sync::cell_select(const phy_interface_rrc_lte::phy_cell_t* new_cell) +bool sync::cell_select(const phy_interface_rrc_lte::phy_cell_t* new_cell, float cfo) { std::unique_lock ul(rrc_mutex); @@ -288,7 +288,7 @@ bool sync::cell_select(const phy_interface_rrc_lte::phy_cell_t* new_cell) /* Reconfigure cell if necessary */ if (new_cell != nullptr) { cell.id = new_cell->pci; - if (!set_cell()) { + if (not set_cell(cfo)) { Error("Cell Select: Reconfiguring cell\n"); return ret; } @@ -500,6 +500,7 @@ void sync::run_thread() // Set CFO for all Carriers for (uint32_t cc = 0; cc < worker_com->args->nof_carriers; cc++) { worker->set_cfo(cc, get_tx_cfo()); + worker_com->avg_cfo_hz[cc] = srslte_ue_sync_get_cfo(&ue_sync); } worker->set_tti(tti); @@ -525,16 +526,6 @@ void sync::run_thread() worker_com->semaphore.push(worker); workers_pool->start_worker(worker); - // Save signal for Intra-frequency measurement - if (srslte_cell_isvalid(&cell)) { - for (size_t i = 0; i < intra_freq_meas.size(); i++) { - intra_freq_meas[i]->write(tti, worker->get_buffer(i, 0), SRSLTE_SF_LEN_PRB(cell.nof_prb)); - - // Update RX gain - intra_freq_meas[i]->set_rx_gain_offset(worker_com->rx_gain_offset); - } - } - break; case 0: Warning("SYNC: Out-of-sync detected in PSS/SSS\n"); @@ -695,7 +686,8 @@ void sync::set_ue_sync_opts(srslte_ue_sync_t* q, float cfo) worker_com->args->cfo_loop_pss_conv); // Disable CP based CFO estimation during find - if (cfo != 0) { + if (isnormal(cfo)) { + srslte_ue_sync_cfo_reset(q, cfo); q->cfo_current_value = cfo / 15000; q->cfo_is_copied = true; q->cfo_correct_enable_find = true; @@ -720,7 +712,7 @@ void sync::set_ue_sync_opts(srslte_ue_sync_t* q, float cfo) srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t)sss_alg); } -bool sync::set_cell() +bool sync::set_cell(float cfo) { if (!phy_state.is_idle()) { Warning("Can not change Cell while not in IDLE\n"); @@ -752,7 +744,7 @@ bool sync::set_cell() } // Set options defined in expert section - set_ue_sync_opts(&ue_sync, search_p.get_last_cfo()); + set_ue_sync_opts(&ue_sync, cfo); // Reset ue_sync and set CFO/gain from search procedure srslte_ue_sync_reset(&ue_sync); @@ -877,6 +869,16 @@ int sync::radio_recv_fnc(srslte::rf_buffer_t& data, uint32_t nsamples, srslte_ti channel_emulator->run(data.to_cf_t(), data.to_cf_t(), nsamples, *rx_time); } + // Save signal for Intra-frequency measurement + if (srslte_cell_isvalid(&cell)) { + for (uint32_t i = 0; (uint32_t)i < intra_freq_meas.size(); i++) { + intra_freq_meas[i]->write(tti, data.get(i, 0, worker_com->args->nof_rx_ant), SRSLTE_SF_LEN_PRB(cell.nof_prb)); + + // Update RX gain + intra_freq_meas[i]->set_rx_gain_offset(worker_com->rx_gain_offset); + } + } + int offset = nsamples - current_sflen; if (abs(offset) < 10 && offset != 0) { next_radio_offset = offset; diff --git a/srsue/src/stack/rrc/rrc.cc b/srsue/src/stack/rrc/rrc.cc index 5cf448751..fae2e6944 100644 --- a/srsue/src/stack/rrc/rrc.cc +++ b/srsue/src/stack/rrc/rrc.cc @@ -415,6 +415,7 @@ void rrc::process_new_cell_meas(const std::vector& meas) if (c != nullptr) { c->set_rsrp(measurements->rsrp_filter(m.rsrp, c->get_rsrp())); c->set_rsrq(measurements->rsrq_filter(m.rsrq, c->get_rsrq())); + c->set_cfo(m.cfo_hz); } else { // or just set initial value neighbour_added = add_neighbour_cell(m); @@ -423,8 +424,11 @@ void rrc::process_new_cell_meas(const std::vector& meas) if (m.earfcn == 0) { rrc_log->info("MEAS: New measurement serving cell: rsrp=%.2f dBm.\n", m.rsrp); } else { - rrc_log->info( - "MEAS: New measurement neighbour cell: earfcn=%d, pci=%d, rsrp=%.2f dBm.\n", m.earfcn, m.pci, m.rsrp); + rrc_log->info("MEAS: New measurement neighbour cell: earfcn=%d, pci=%d, rsrp=%.2f dBm, cfo=%+.1f Hz\n", + m.earfcn, + m.pci, + m.rsrp, + m.cfo_hz); } } @@ -541,11 +545,8 @@ void rrc::set_serving_cell(phy_interface_rrc_lte::phy_cell_t phy_cell, bool disc } // Set new serving cell serving_cell = std::move(new_serving_cell); - rrc_log->info("Setting serving cell earfcn=%d, PCI=%d, rsrp=%.2f, nof_neighbours=%zd\n", - serving_cell->get_earfcn(), - serving_cell->get_pci(), - serving_cell->get_rsrp(), - neighbour_cells.size()); + rrc_log->info( + "Setting serving cell %s, nof_neighbours=%zd\n", serving_cell->to_string().c_str(), neighbour_cells.size()); } else { rrc_log->error("Setting serving cell: Unknown cell with earfcn=%d, PCI=%d\n", phy_cell.earfcn, phy_cell.pci); } @@ -653,6 +654,7 @@ bool rrc::add_neighbour_cell(phy_meas_t meas) unique_cell_t c = unique_cell_t(new cell_t(phy_cell)); c.get()->set_rsrp(meas.rsrp); c.get()->set_rsrq(meas.rsrq); + c.get()->set_cfo(meas.cfo_hz); return add_neighbour_cell(std::move(c)); } @@ -1083,7 +1085,7 @@ bool rrc::ho_prepare() } cell_t* target_cell = get_neighbour_cell_handle(target_earfcn, mob_ctrl_info->target_pci); - if (!phy->cell_select(&target_cell->phy_cell)) { + if (not phy->cell_select(&target_cell->phy_cell, target_cell->get_cfo_hz())) { rrc_log->error("Could not synchronize with target cell %s. Removing cell and trying to return to source %s\n", target_cell->to_string().c_str(), serving_cell->to_string().c_str());