From 0c2f0ac03844e4879978d994b8b4b1bc0dba1d97 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 7 Feb 2018 18:36:06 +0100
Subject: [PATCH] Fixed srsue freq_offset compensation for UL

---
 lib/include/srslte/radio/radio.h |  1 +
 lib/src/radio/radio.cc           |  5 ++++
 srsue/hdr/phy/phch_worker.h      |  2 ++
 srsue/src/phy/phch_worker.cc     | 47 ++++++++++++++++++++++++++++----
 4 files changed, 50 insertions(+), 5 deletions(-)

diff --git a/lib/include/srslte/radio/radio.h b/lib/include/srslte/radio/radio.h
index 6ff0c5100..454f1578f 100644
--- a/lib/include/srslte/radio/radio.h
+++ b/lib/include/srslte/radio/radio.h
@@ -101,6 +101,7 @@ namespace srslte {
       void set_tx_freq(double freq);
       void set_rx_freq(double freq);
 
+      double get_freq_offset();
       double get_tx_freq();
       double get_rx_freq();
 
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index f592db71f..f3baad4a5 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -313,6 +313,11 @@ double radio::get_rx_freq()
   return rx_freq;
 }
 
+double radio::get_freq_offset()
+{
+  return freq_offset;
+}
+
 double radio::get_tx_freq()
 {
   return tx_freq; 
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index d73214514..c154ff514 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -70,6 +70,8 @@ public:
   void start_plot();
 
   float get_ref_cfo();
+  float get_cfo();
+  float get_ul_cfo();
 
 private:
   /* Inherited from thread_pool::worker. Function called every subframe to run the DL/UL processing */
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 6951574f5..f7e1381f8 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -203,6 +203,25 @@ float phch_worker::get_ref_cfo()
   return srslte_chest_dl_get_cfo(&ue_dl.chest);
 }
 
+float phch_worker::get_cfo()
+{
+  return cfo;
+}
+
+float phch_worker::get_ul_cfo() {
+  srslte::radio *radio = phy->get_radio();
+
+  if (radio->get_freq_offset() != 0.0f) {
+    /* Compensates the radio frequency offset applied equally to DL and UL */
+    const float ul_dl_ratio = (float) radio->get_tx_freq() / (float) radio->get_rx_freq();
+    const float offset_hz = (float) radio->get_freq_offset() * (1.0f - ul_dl_ratio);
+    return cfo - offset_hz / (15000);
+  } else {
+    return cfo;
+  }
+
+}
+
 void phch_worker::work_imp()
 {
   if (!cell_initiated) {
@@ -324,7 +343,7 @@ void phch_worker::work_imp()
   }
 
   /* Set UL CFO before transmission */  
-  srslte_ue_ul_set_cfo(&ue_ul, cfo);
+  srslte_ue_ul_set_cfo(&ue_ul, get_ul_cfo());
 
   /* Transmit PUSCH, PUCCH or SRS */
   bool signal_ready = false; 
@@ -1476,6 +1495,13 @@ plot_scatter_t pconst;
 float tmp_plot[SCATTER_PDSCH_BUFFER_LEN];
 cf_t  tmp_plot2[SRSLTE_SF_LEN_RE(SRSLTE_MAX_PRB, SRSLTE_CP_NORM)];
 
+#define CFO_PLOT_LEN 0 /* Set to non zero for enabling CFO plot */
+#if CFO_PLOT_LEN > 0
+static plot_real_t    pcfo;
+static uint32_t icfo = 0;
+static float cfo_buffer[CFO_PLOT_LEN];
+#endif /* CFO_PLOT_LEN > 0 */
+
 void *plot_thread_run(void *arg) {
   srsue::phch_worker *worker = (srsue::phch_worker*) arg; 
 
@@ -1500,10 +1526,14 @@ void *plot_thread_run(void *arg) {
 
   plot_scatter_addToWindowGrid(&pconst, (char*)"srsue", 0, worker->get_rx_nof_antennas());
 
+#if CFO_PLOT_LEN > 0
+  plot_real_init(&pcfo);
+  plot_real_setTitle(&pcfo, (char*) "CFO (Hz)");
+  plot_real_setLabels(&pcfo, (char *) "Time", (char *) "Hz");
+  plot_real_setYAxisScale(&pcfo, -4000, 4000);
 
-
-
-
+  plot_scatter_addToWindowGrid(&pcfo, (char*)"srsue", 1, worker->get_rx_nof_antennas());
+#endif /* CFO_PLOT_LEN > 0 */
 
   int n; 
   int readed_pdsch_re=0; 
@@ -1527,7 +1557,14 @@ void *plot_thread_run(void *arg) {
       }
       readed_pdsch_re = 0; 
     }
-  }  
+
+#if CFO_PLOT_LEN > 0
+    cfo_buffer[icfo] = worker->get_cfo() * 15000.0f;
+    icfo = (icfo + 1)%CFO_PLOT_LEN;
+    plot_real_setNewData(&pcfo, cfo_buffer, CFO_PLOT_LEN);
+#endif /* CFO_PLOT_LEN > 0 */
+
+  }
   return NULL;
 }