Improved CFO loop. Fixed issue with sinusoid tolerance

lib/examples/pdsch_ue.c

@@ -546,7 +546,7 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, prog_args.enable_cfo_ref, 0xff, 0.005);
+  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, prog_args.enable_cfo_ref, 1023);
   srslte_chest_dl_average_subframe(&ue_dl.chest, prog_args.average_subframe);
 
   /* Configure downlink receiver for the SI-RNTI since will be the only one we'll use */

lib/include/srslte/phy/ch_estimation/chest_dl.h

@@ -87,7 +87,6 @@ typedef struct {
 
   bool     cfo_estimate_enable;
   uint32_t cfo_estimate_sf_mask;
-  float    cfo_ema;
 
   /* Use PSS for noise estimation in LS linear interpolation mode */
   cf_t pss_signal[SRSLTE_PSS_LEN];
@@ -153,8 +152,7 @@ SRSLTE_API int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q,
 
 SRSLTE_API void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q,
                                                     bool enable,
-                                                    uint32_t mask,
+                                                    uint32_t mask);
-                                                    float ema);
 
 SRSLTE_API void srslte_chest_dl_average_subframe(srslte_chest_dl_t *q,
                                                  bool enable);

lib/include/srslte/phy/sync/cfo.h

@@ -40,9 +40,6 @@
 #include "srslte/config.h"
 #include "srslte/phy/utils/cexptab.h"
 
-/** If the frequency is changed more than the tolerance, a new table is generated */
-#define SRSLTE_CFO_TOLERANCE    0.00001
-
 #define SRSLTE_CFO_CEXPTAB_SIZE 4096
 
 typedef struct SRSLTE_API {

lib/include/srslte/phy/sync/sync.h

@@ -102,6 +102,7 @@ typedef struct SRSLTE_API {
   bool cfo_i_initiated;
 
   float cfo_cp_mean;
+  float cfo_pss;
   float cfo_pss_mean;
   int   cfo_i_value;
 

lib/include/srslte/phy/ue/ue_sync.h

@@ -62,6 +62,16 @@
 #include "srslte/phy/io/filesource.h"
 
 
+#define DEFAULT_CFO_BW_PSS  0.05
+#define DEFAULT_CFO_PSS_MIN 400  // typical bias of PSS estimation.
+#define DEFAULT_CFO_BW_REF  0.01
+#define DEFAULT_CFO_REF_MIN 0    // typical bias of REF estimation
+#define DEFAULT_CFO_REF_MAX DEFAULT_CFO_PSS_MIN  // Maximum detection offset of REF based estimation
+
+#define DEFAULT_PSS_STABLE_TIMEOUT     20  // Time after which the PSS is considered to be stable and we accept REF-CFO
+
+#define DEFAULT_CFO_EMA_TRACK 0.05
+
 typedef enum SRSLTE_API { SF_FIND, SF_TRACK} srslte_ue_sync_state_t;
 
 //#define MEASURE_EXEC_TIME 

lib/src/phy/ch_estimation/chest_dl.c

@@ -467,6 +467,10 @@ float chest_estimate_cfo(srslte_chest_dl_t *q)
 }
 
 void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id, srslte_sf_t ch_mode){
+  if (q->cfo_estimate_enable && ((1<<sf_idx) & q->cfo_estimate_sf_mask)) {
+    q->cfo = chest_estimate_cfo(q);
+  }
+
   if (ce != NULL) {
     /* Smooth estimates (if applicable) and interpolate */
     if (q->smooth_filter_len == 0 || (q->smooth_filter_len == 3 && q->smooth_filter[0] == 0)) {
@@ -490,10 +494,6 @@ void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, ui
     } 
   }
 
-  if (q->cfo_estimate_enable && ((1<<sf_idx) & q->cfo_estimate_sf_mask)) {
-    q->cfo = SRSLTE_VEC_EMA(chest_estimate_cfo(q), q->cfo, q->cfo_ema);
-  }
-    
   /* Compute RSRP for the channel estimates in this port */
   uint32_t npilots = SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
   float energy = cabsf(srslte_vec_acc_cc(q->pilot_estimates, npilots)/npilots);
@@ -581,9 +581,8 @@ void srslte_chest_dl_average_subframe(srslte_chest_dl_t *q, bool enable)
   q->average_subframe = enable;
 }
 
-void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q, bool enable, uint32_t mask, float ema)
+void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q, bool enable, uint32_t mask)
 {
-  q->cfo_ema              = ema;
   q->cfo_estimate_enable  = enable;
   q->cfo_estimate_sf_mask = mask;
 }

lib/src/phy/sync/cfo.c

@@ -45,7 +45,7 @@ int srslte_cfo_init(srslte_cfo_t *h, uint32_t nsamples) {
   if (!h->cur_cexp) {
     goto clean;
   }
-  h->tol = SRSLTE_CFO_TOLERANCE;
+  h->tol = 0;
   h->last_freq = 0;
   h->nsamples = nsamples;
   h->max_samples = nsamples;

lib/src/phy/sync/pss.c

@@ -120,6 +120,7 @@ int srslte_pss_init_fft_offset_decim(srslte_pss_t *q,
     buffer_size = fft_size + frame_size + 1;
 
     q->filter_pss_enable = false;
+    q->chest_on_filter   = false;
 
     if(q->decimate > 1) {
         int filter_order = 3;

lib/src/phy/sync/sync.c

@@ -475,7 +475,7 @@ srslte_pss_t* srslte_sync_get_cur_pss_obj(srslte_sync_t *q)
 static float cfo_cp_estimate(srslte_sync_t *q, const cf_t *input)
 {
   uint32_t cp_offset = 0; 
-  cp_offset = srslte_cp_synch(&q->cp_synch, input, q->max_offset, 7, SRSLTE_CP_LEN_NORM(1,q->fft_size));
+  cp_offset = srslte_cp_synch(&q->cp_synch, input, q->max_offset, 1, SRSLTE_CP_LEN_NORM(1,q->fft_size));
   cf_t cp_corr_max = srslte_cp_synch_corr_output(&q->cp_synch, cp_offset);
   float cfo = -carg(cp_corr_max) / M_PI / 2; 
   return cfo; 
@@ -612,16 +612,16 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, const cf_t *input, uin
         }
 
         // PSS-based CFO estimation
-        float cfo_pss = srslte_pss_cfo_compute(&q->pss, pss_ptr);
+        q->cfo_pss = srslte_pss_cfo_compute(&q->pss, pss_ptr);
         if (!q->cfo_pss_is_set) {
-          q->cfo_pss_mean   = cfo_pss;
+          q->cfo_pss_mean   = q->cfo_pss;
           q->cfo_pss_is_set = true;
-        } else if (15000*fabsf(cfo_pss) < MAX_CFO_PSS_OFFSET) {
+        } else if (15000*fabsf(q->cfo_pss) < MAX_CFO_PSS_OFFSET) {
-          q->cfo_pss_mean = SRSLTE_VEC_EMA(cfo_pss, q->cfo_pss_mean, q->cfo_ema_alpha);
+          q->cfo_pss_mean = SRSLTE_VEC_EMA(q->cfo_pss, q->cfo_pss_mean, q->cfo_ema_alpha);
         }
 
         INFO("PSS-CFO: filter=%s, estimated=%f, mean=%f\n",
-             q->pss_filtering_enabled?"yes":"no", cfo_pss, q->cfo_pss_mean);
+             q->pss_filtering_enabled?"yes":"no", q->cfo_pss, q->cfo_pss_mean);
 
       }
 

lib/src/phy/ue/ue_sync.c

@@ -29,7 +29,7 @@
 #include <strings.h>
 #include <assert.h>
 #include <unistd.h>
-#include <srslte/srslte.h>
+#include "srslte/srslte.h"
 
 
 #include "srslte/phy/ue/ue_sync.h"
@@ -47,14 +47,6 @@
 #define DEFAULT_SAMPLE_OFFSET_CORRECT_PERIOD  0
 #define DEFAULT_SFO_EMA_COEFF                 0.1
 
-#define DEFAULT_CFO_BW      0.2
-#define DEFAULT_CFO_PSS_MIN 500  // typical bias of PSS estimation.
-#define DEFAULT_CFO_REF_MIN 0    // typical bias of REF estimation
-#define DEFAULT_CFO_REF_MAX 500  // Maximum detection offset of REF based estimation
-
-#define DEFAULT_PSS_STABLE_TIMEOUT     100  // Time after which the PSS is considered to be stable and we accept REF-CFO
-
-#define DEFAULT_CFO_EMA_TRACK 0.1
 
 cf_t dummy_buffer0[15*2048/2];
 cf_t dummy_buffer1[15*2048/2];
@@ -226,8 +218,8 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
     q->cfo_ref_max = DEFAULT_CFO_REF_MAX;
     q->cfo_ref_min = DEFAULT_CFO_REF_MIN;
     q->cfo_pss_min = DEFAULT_CFO_PSS_MIN;
-    q->cfo_loop_bw_pss = DEFAULT_CFO_BW;
+    q->cfo_loop_bw_pss = DEFAULT_CFO_BW_PSS;
-    q->cfo_loop_bw_ref = DEFAULT_CFO_BW;
+    q->cfo_loop_bw_ref = DEFAULT_CFO_BW_REF;
     q->cfo_correct_enable = true;
 
     q->pss_stable_cnt     = 0;
@@ -679,6 +671,8 @@ int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q, cf_t *input_buffer) {
   return srslte_ue_sync_zerocopy_multi(q, _input_buffer);
 }
 
+int track_time, find_time;
+
 /* Returns 1 if the subframe is synchronized in time, 0 otherwise */
 int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE_MAX_PORTS]) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
@@ -723,10 +717,16 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
         fprintf(stderr, "Error receiving samples\n");
         return SRSLTE_ERROR;
       }
-
+      int n;
+      struct timeval t[3];
       switch (q->state) {
         case SF_FIND:
-          switch(srslte_sync_find(&q->sfind, input_buffer[0], 0, &q->peak_idx)) {
+          gettimeofday(&t[1], NULL);
+          n = srslte_sync_find(&q->sfind, input_buffer[0], 0, &q->peak_idx);
+          gettimeofday(&t[2], NULL);
+          get_time_interval(t);
+          find_time = t[0].tv_usec;
+          switch(n) {
             case SRSLTE_SYNC_ERROR: 
               ret = SRSLTE_ERROR; 
               fprintf(stderr, "Error finding correlation peak (%d)\n", ret);
@@ -781,9 +781,14 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
              * In tracking phase, the subframe carrying the PSS is always the last one of the frame
              */
             track_idx = 0;
-            switch(srslte_sync_find(&q->strack, input_buffer[0], 
+            gettimeofday(&t[1], NULL);
+            int n = srslte_sync_find(&q->strack, input_buffer[0],
                                      q->frame_len - q->sf_len/2 - q->fft_size - q->strack.max_offset/2,
-                                    &track_idx)) 
+                                     &track_idx);
+            gettimeofday(&t[2], NULL);
+            get_time_interval(t);
+            track_time = t[0].tv_usec;
+            switch(n)
             {
               case SRSLTE_SYNC_ERROR:
                 fprintf(stderr, "Error tracking correlation peak\n");

lib/src/phy/ue/ue_ul.c

@@ -38,7 +38,7 @@
 
 #define MAX_SFLEN     SRSLTE_SF_LEN(srslte_symbol_sz(max_prb))
 
-#define DEFAULT_CFO_TOL   0.0 // Hz
+#define DEFAULT_CFO_TOL   1.0 // Hz
 
 int srslte_ue_ul_init(srslte_ue_ul_t *q,
                       cf_t *out_buffer,

srsue/hdr/phy/phch_worker.h

@@ -67,6 +67,8 @@ public:
   int read_pdsch_d(cf_t *pdsch_d);
   void start_plot();
 
+  float get_ref_cfo();
+
 private:
   /* Inherited from thread_pool::worker. Function called every subframe to run the DL/UL processing */
   void work_imp();

srsue/src/main.cc

@@ -37,6 +37,7 @@
 #include <boost/program_options/parsers.hpp>
 
 #include "ue.h"
+#include "srslte/srslte.h"
 #include "metrics_stdout.h"
 #include "metrics_csv.h"
 #include "srslte/common/metrics_hub.h"
@@ -201,40 +202,42 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      "Enables integer CFO estimation and correction.")
 
     ("expert.cfo_correct_tol_hz",
-     bpo::value<float>(&args->expert.phy.cfo_correct_tol_hz)->default_value(0.0),
+     bpo::value<float>(&args->expert.phy.cfo_correct_tol_hz)->default_value(1.0),
      "Tolerance (in Hz) for digital CFO compensation (needs to be low if average_subframe_enabled=true.")
 
     ("expert.cfo_pss_ema",
-     bpo::value<float>(&args->expert.phy.cfo_pss_ema)->default_value(0.01),
+     bpo::value<float>(&args->expert.phy.cfo_pss_ema)->default_value(DEFAULT_CFO_EMA_TRACK),
      "CFO Exponential Moving Average coefficient for PSS estimation during TRACK.")
 
+    /* REF EMA is currently not used
     ("expert.cfo_ref_ema",
      bpo::value<float>(&args->expert.phy.cfo_ref_ema)->default_value(0.01),
      "CFO Exponential Moving Average coefficient for RS estimation after PSS acquisition")
+    */
 
     ("expert.cfo_ref_mask",
      bpo::value<uint32_t>(&args->expert.phy.cfo_ref_mask)->default_value(1023),
      "Bitmask for subframes on which to run RS estimation (set to 0 to disable, default all sf)")
 
     ("expert.cfo_loop_bw_pss",
-     bpo::value<float>(&args->expert.phy.cfo_loop_bw_pss)->default_value(0.05),
+     bpo::value<float>(&args->expert.phy.cfo_loop_bw_pss)->default_value(DEFAULT_CFO_BW_PSS),
      "CFO feedback loop bandwidth for samples from PSS")
 
     ("expert.cfo_loop_bw_ref",
-     bpo::value<float>(&args->expert.phy.cfo_loop_bw_ref)->default_value(0.01),
+     bpo::value<float>(&args->expert.phy.cfo_loop_bw_ref)->default_value(DEFAULT_CFO_BW_REF),
      "CFO feedback loop bandwidth for samples from RS")
 
     ("expert.cfo_loop_pss_tol",
-     bpo::value<float>(&args->expert.phy.cfo_loop_pss_tol)->default_value(300),
+     bpo::value<float>(&args->expert.phy.cfo_loop_pss_tol)->default_value(DEFAULT_CFO_PSS_MIN),
      "Tolerance (in Hz) of the PSS estimation method. Below this value, PSS estimation does not feeds back the loop"
        "and RS estimations are used instead (when available)")
 
     ("expert.cfo_loop_ref_min",
-     bpo::value<float>(&args->expert.phy.cfo_loop_ref_min)->default_value(0),
+     bpo::value<float>(&args->expert.phy.cfo_loop_ref_min)->default_value(DEFAULT_CFO_REF_MIN),
      "Tolerance (in Hz) of the RS estimation method. Below this value, RS estimation does not feeds back the loop")
 
     ("expert.cfo_loop_pss_conv",
-     bpo::value<uint32_t>(&args->expert.phy.cfo_loop_pss_conv)->default_value(20),
+     bpo::value<uint32_t>(&args->expert.phy.cfo_loop_pss_conv)->default_value(DEFAULT_PSS_STABLE_TIMEOUT),
      "After the PSS estimation is below cfo_loop_pss_tol for cfo_loop_pss_timeout times consecutively, RS adjustments are allowed.")
 
     ("expert.sic_pss_enabled",

srsue/src/phy/phch_recv.cc

@@ -539,6 +539,7 @@ double phch_recv::set_rx_gain(double gain) {
 void phch_recv::run_thread()
 {
   phch_worker *worker = NULL;
+  phch_worker *last_worker = NULL;
   cf_t *buffer[SRSLTE_MAX_PORTS] = {NULL};
   uint32_t sf_idx = 0;
   phy_state  = IDLE;
@@ -637,6 +638,15 @@ void phch_recv::run_thread()
           switch(srslte_ue_sync_zerocopy_multi(&ue_sync, buffer)) {
             case 1:
 
+              if (last_worker) {
+                Warning("SF: cfo=%7.1f Hz, ref=%f Hz, pss=%f Hz\n",
+                        srslte_ue_sync_get_cfo(&ue_sync),
+                     15000*last_worker->get_ref_cfo(),
+                     15000*ue_sync.strack.cfo_pss_mean);
+              }
+
+              last_worker = worker;
+
               Debug("SYNC:  Worker %d synchronized\n", worker->get_id());
 
               metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);

srsue/src/phy/phch_worker.cc

@@ -132,7 +132,7 @@ bool phch_worker::init(uint32_t max_prb, srslte::log *log_h, chest_feedback_itf
   }
 
   srslte_chest_dl_average_subframe(&ue_dl.chest, phy->args->average_subframe_enabled);
-  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, phy->args->cfo_ref_mask!=0, phy->args->cfo_ref_mask, phy->args->cfo_ref_ema);
+  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, phy->args->cfo_ref_mask!=0, phy->args->cfo_ref_mask);
   srslte_ue_ul_set_normalization(&ue_ul, true);
   srslte_ue_ul_set_cfo_enable(&ue_ul, true);
 
@@ -195,6 +195,11 @@ void phch_worker::set_crnti(uint16_t rnti)
   rnti_is_set = true; 
 }
 
+float phch_worker::get_ref_cfo()
+{
+  return srslte_chest_dl_get_cfo(&ue_dl.chest);
+}
+
 void phch_worker::work_imp()
 {
   if (!cell_initiated) {

srsue/ue.conf.example

@@ -202,15 +202,14 @@ enable = false
 
 # CFO related values
 #cfo_integer_enabled = false
-#cfo_correct_tol_hz  = 0
+#cfo_correct_tol_hz  = 1.0
-#cfo_pss_ema         = 0.1
+#cfo_pss_ema         = 0.05
-#cfo_ref_ema         = 0.01
 #cfo_ref_mask        = 1023
 #cfo_loop_bw_pss     = 0.05
 #cfo_loop_bw_ref     = 0.01
-#cfo_loop_pss_tol    = 300
+#cfo_loop_pss_tol    = 400
 #cfo_loop_ref_min    = 0
-#cfo_loop_pss_conv   = 50
+#cfo_loop_pss_conv   = 20
 
 #####################################################################
 # Manual RF calibration