SRSUE: Fix HO initial CFO

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 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_select(const phy_cell_t* cell = nullptr, float cfo_hz = 0.0f) = 0;
-  virtual bool              cell_is_camping()                          = 0;
+  virtual bool              cell_is_camping()                                                  = 0;
 
   virtual void reset() = 0;
 

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);

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);
 

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;

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 {

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;
+  q->cfo_current_value = init_cfo_hz / 15e3f;
-  srslte_sync_cfo_reset(&q->strack);
+  srslte_sync_cfo_reset(&q->strack, init_cfo_hz);
-  srslte_sync_cfo_reset(&q->sfind);
+  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) {

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<uint32_t>& pci) final;
   void meas_stop() final;

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]      = {};

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;

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 = {};
 

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)

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, "

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;

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;

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<std::mutex> 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;

srsue/src/stack/rrc/rrc.cc

@@ -415,6 +415,7 @@ void rrc::process_new_cell_meas(const std::vector<phy_meas_t>& 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<phy_meas_t>& meas)
     if (m.earfcn == 0) {
       rrc_log->info("MEAS:  New measurement serving cell: rsrp=%.2f dBm.\n", m.rsrp);
     } else {
-      rrc_log->info(
+      rrc_log->info("MEAS:  New measurement neighbour cell: earfcn=%d, pci=%d, rsrp=%.2f dBm, cfo=%+.1f Hz\n",
-          "MEAS:  New measurement neighbour cell: earfcn=%d, pci=%d, rsrp=%.2f dBm.\n", m.earfcn, m.pci, m.rsrp);
+                    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",
+    rrc_log->info(
-                  serving_cell->get_earfcn(),
+        "Setting serving cell %s, nof_neighbours=%zd\n", serving_cell->to_string().c_str(), neighbour_cells.size());
-                  serving_cell->get_pci(),
-                  serving_cell->get_rsrp(),
-                  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());