Changed method of frequency estimation

2 files changed, 82 insertions, 8 deletions

diff --git a/lib/receiver/receiver_impl.cc b/lib/receiver/receiver_impl.cc
index 6886106..f55d613 100644
--- a/lib/receiver/receiver_impl.cc
+++ b/lib/receiver/receiver_impl.cc
@@ -26,6 +26,7 @@
 
 #include <gnuradio/io_signature.h>
 #include <gnuradio/math.h>
+#include <volk/volk.h>
 #include <math.h>
 #include <boost/circular_buffer.hpp>
 #include <algorithm>
@@ -34,6 +35,7 @@
 #include <viterbi_detector.h>
 #include <string.h>
 #include <iostream>
+#include <time.h> //!!!
 //#include <iomanip>
 #include <boost/scoped_ptr.hpp>
 
@@ -80,6 +82,10 @@ receiver_impl::receiver_impl(int osr, const std::vector<int> &cell_allocation, c
     d_last_time(0.0)
 {
     int i;
+
+    unsigned int alignment = volk_get_alignment();
+    d_freq_estim_vector = (lv_32fc_t*)volk_malloc(sizeof(lv_32fc_t)*160, alignment);
+    d_freq_estim_result = (lv_32fc_t*)volk_malloc(sizeof(lv_32fc_t)*1, alignment);
                                                                       //don't send samples to the receiver until there are at least samples for one
     set_output_multiple(floor((TS_BITS + 2 * GUARD_PERIOD) * d_OSR)); // burst and two gurad periods (one gurard period is an arbitrary overlap)
     gmsk_mapper(SYNC_BITS, N_SYNC_BITS, d_sch_training_seq, gr_complex(0.0, -1.0));
@@ -100,6 +106,8 @@ receiver_impl::receiver_impl(int osr, const std::vector<int> &cell_allocation, c
  */
 receiver_impl::~receiver_impl()
 {
+    volk_free(d_freq_estim_vector);
+    volk_free(d_freq_estim_result);
 }
 
 int
@@ -237,7 +245,6 @@ receiver_impl::work(int noutput_items,
                     const unsigned first_sample = ceil((GUARD_PERIOD + 2 * TAIL_BITS) * d_OSR) + 1;
                     const unsigned last_sample = first_sample + USEFUL_BITS * d_OSR - TAIL_BITS * d_OSR;
                     double freq_offset_tmp = compute_freq_offset(input, first_sample, last_sample);       //extract frequency offset from it
-
                     send_burst(d_burst_nr, fc_fb, GSMTAP_BURST_FCCH, input_nr);
 
                     pmt::pmt_t msg = pmt::make_tuple(pmt::mp("freq_offset"),pmt::from_double(freq_offset_tmp-d_freq_offset_setting),pmt::mp("synchronized"));
@@ -533,22 +540,83 @@ bool receiver_impl::find_fcch_burst(const gr_complex *input, const int nitems, d
     return result;
 }
 
-double receiver_impl::compute_freq_offset(const gr_complex * input, unsigned first_sample, unsigned last_sample)
+double receiver_impl::estim_freq_norm(const gr_complex * input, unsigned first_sample, unsigned last_sample) //another frequency estimator
 {
-    double phase_sum = 0;
+
     unsigned ii;
 
-    for (ii = first_sample; ii < last_sample; ii++)
+    gr_complex sum = 0;
+
+    for (ii = first_sample; ii < last_sample-d_OSR; ii=ii+d_OSR)
     {
-        double phase_diff = compute_phase_diff(input[ii], input[ii-1]) - (M_PI / 2) / d_OSR;
-        phase_sum += phase_diff;
+        sum += input[ii+d_OSR] * conj(input[ii]);
     }
 
-    double phase_offset = phase_sum / (last_sample - first_sample);
-    double freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);
+    return fast_atan2f(imag(sum), real(sum))/(2*M_PI);
+}
+
+double receiver_impl::estim_freq_norm2(const gr_complex * input, unsigned first_sample, unsigned last_sample) //another frequency estimator - faster one
+{
+
+    unsigned ii;
+
+    int N = (last_sample-first_sample)/d_OSR;
+    
+    for (unsigned ii = 0; ii < N; ii++)
+    {
+        d_freq_estim_vector[ii] = input[first_sample+ii*d_OSR];
+    }
+
+    volk_32fc_x2_conjugate_dot_prod_32fc(d_freq_estim_result, d_freq_estim_vector+1, d_freq_estim_vector, N-1);
+    
+    return fast_atan2f(imag(d_freq_estim_result[0]), real(d_freq_estim_result[0]))/(2*M_PI);
+}
+
+
+double receiver_impl::compute_freq_offset(const gr_complex * input, unsigned first_sample, unsigned last_sample)
+{
+    float freq_norm = estim_freq_norm2(input, first_sample, last_sample);
+
+//    using namespace std;
+//    clock_t begin = clock();
+//  
+//    for(int ii=0;ii<500;ii++){
+//        float dupa = estim_freq_norm2(input, first_sample, last_sample);
+//    }
+//    clock_t end = clock();
+//    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;  
+//    std::cout << "elapsed_secs " << elapsed_secs << std::endl;
+
+//    begin = clock();
+//  
+//    for(int ii=0;ii<500;ii++){
+//        float dupa = estim_freq_norm(input, first_sample, last_sample);
+//    }
+//    end = clock();
+//    elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;  
+//    std::cout << "elapsed_secs_old " << elapsed_secs << std::endl;
+  
+    float freq_offset = (freq_norm - 0.25) * 1625000.0/6.0;
+    
     return freq_offset;
 }
 
+//double receiver_impl::compute_freq_offset(const gr_complex * input, unsigned first_sample, unsigned last_sample)
+//{
+//    double phase_sum = 0;
+//    unsigned ii;
+
+//    for (ii = first_sample; ii < last_sample; ii++)
+//    {
+//        double phase_diff = compute_phase_diff(input[ii], input[ii-1]) - (M_PI / 2) / d_OSR;
+//        phase_sum += phase_diff;
+//    }
+
+//    double phase_offset = phase_sum / (last_sample - first_sample);
+//    double freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);
+//    return freq_offset;
+//}
+
 inline float receiver_impl::compute_phase_diff(gr_complex val1, gr_complex val2)
 {
     gr_complex conjprod = val1 * conj(val2);
diff --git a/lib/receiver/receiver_impl.h b/lib/receiver/receiver_impl.h
index c8d71f9..eb406f3 100644
--- a/lib/receiver/receiver_impl.h
+++ b/lib/receiver/receiver_impl.h
@@ -36,6 +36,10 @@ namespace gr {
      private:
         unsigned int d_c0_burst_start;
         float d_c0_signal_dbm;
+        
+        lv_32fc_t* d_freq_estim_vector;//  = (lv_32fc_t*)volk_malloc(sizeof(lv_32fc_t)*160, alignment);
+        lv_32fc_t* d_freq_estim_result;// = (lv_32fc_t*)volk_malloc(sizeof(lv_32fc_t)*1, alignment);
+        
         /**@name Configuration of the receiver */
         //@{
         const int d_OSR; ///< oversampling ratio
@@ -108,6 +112,8 @@ namespace gr {
          */
         double compute_freq_offset(const gr_complex * input, unsigned first_sample, unsigned last_sample);
 
+        double estim_freq_norm(const gr_complex * input, unsigned first_sample, unsigned last_sample); //another frequency estimator
+        double estim_freq_norm2(const gr_complex * input, unsigned first_sample, unsigned last_sample); //another frequency estimator        
         /** Computes angle between two complex numbers
          *
          * @param val1 first complex number