Corrected precision of FCCH search function

2 files changed, 220 insertions, 69 deletions

diff --git a/src/lib/gsm_receiver_cf.cc b/src/lib/gsm_receiver_cf.cc
index 206bd5e..4f43f2b 100644
--- a/src/lib/gsm_receiver_cf.cc
+++ b/src/lib/gsm_receiver_cf.cc
@@ -27,14 +27,18 @@
 #include <gsm_receiver_cf.h>
 #include <gr_io_signature.h>
 #include <gr_math.h>
+#include <math.h>
 #include <gsm_constants.h>
 #include <Assert.h>
+#include <algorithm>
 
+#define SAFETY_MARGIN 50
+#define BUFFER_SIZE (FCCH_HITS_NEEDED)
 
 gsm_receiver_cf_sptr
-gsm_make_receiver_cf(int osr)
+gsm_make_receiver_cf(gr_feval_dd *tuner, int osr)
 {
-  return gsm_receiver_cf_sptr(new gsm_receiver_cf(osr));
+  return gsm_receiver_cf_sptr(new gsm_receiver_cf(tuner, osr));
 }
 
 static const int MIN_IN = 1; // mininum number of input streams
@@ -45,11 +49,19 @@ static const int MAX_OUT = 1; // maximum number of output streams
 /*
  * The private constructor
  */
-gsm_receiver_cf::gsm_receiver_cf(int osr)
+gsm_receiver_cf::gsm_receiver_cf(gr_feval_dd *tuner, int osr)
     : gr_block("gsm_receiver",
                gr_make_io_signature(MIN_IN, MAX_IN, sizeof(gr_complex)),
                gr_make_io_signature(MIN_OUT, MAX_OUT, 142 * sizeof(float))),
-    d_counter(0)
+    d_osr(osr),
+    d_tuner(tuner),
+    d_prev_freq_offset(0),
+    d_phase_diff_buffer(BUFFER_SIZE),
+    d_counter(0),
+    d_x_temp(0),
+    d_x2_temp(0),
+    d_fcch_count(0),
+    d_state(fcch_search)
 {
 }
 
@@ -73,76 +85,199 @@ gsm_receiver_cf::general_work(int noutput_items,
 {
   const gr_complex *in = (const gr_complex *) input_items[0];
   float *out = (float *) output_items[0];
+  int produced_out;
 
-//  DCOUT(ninput_items[0]);
-//  DCOUT(noutput_items);
+  switch (d_state) {
 
+    case fcch_search:
 
-  d_return = 0; //!
-  int start_pos = find_fcch_burst(in, ninput_items[0]);
-  for (int i = 0; i < TS_BITS; i++) {
-    out[i] = d_phase_diff_buffer[i+start_pos-USEFUL_BITS];
+      if (find_fcch_burst(in, ninput_items[0])) {
+        produced_out = 1;
+        d_state = fcch_search;
+      } else {
+        produced_out = 0;
+        d_state = fcch_search;
+      }
+
+      break;
+
+    case sch_search:
+      break;
   }
-  consume_each(start_pos);
-  d_counter += start_pos;
-  return d_return;
+
+//  for (int i = 0; i < TS_BITS; i++) {
+//    out[i] = d_phase_diff_buffer[i+start_pos-USEFUL_BITS];
+//  }
+
+  return produced_out;
 }
 
-int gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
+bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
 {
-  for (int i = 0; (i < nitems - 1) && (i < BUFFER_SIZE); i++) {
-    gr_complex conjprod = in[i+1] * conj(in[i]);
-    float diff_angle = gr_fast_atan2f(imag(conjprod), real(conjprod));
-    d_phase_diff_buffer[i] = diff_angle;
-  }
+  float phase_diff = 0;
+  gr_complex conjprod;
+  int hit_count, miss_count, start_pos;
+  float min_phase_diff, max_phase_diff, lowest_max_min_diff;
+  float sum, best_sum;
 
-  int hit_count = 0;
-  int miss_count = 0;
-  int start_pos = -1;
+  int to_consume = 0;
+  int i = 0;
+  bool end = false;
+  bool result;
 
-  d_return = 0;//!
-  
-  for (int i = 0; ; i++) {
-    if (i > nitems - USEFUL_BITS - 1) {
-      start_pos = nitems - USEFUL_BITS - 1;
-      break;
-    }
+  circular_buffer_float::iterator buffer_iter;
 
-    if (d_phase_diff_buffer[i] > 0) {
-      if (! hit_count) {
-        start_pos = i+USEFUL_BITS;
-      }
-      hit_count++;
-    } else if (hit_count >= FCCH_HITS_NEEDED) {
-      DCOUT("znalezione fcch na pozycji" << d_counter+start_pos-USEFUL_BITS);
-      d_return = 1;//!
-      break;
-    } else if (++miss_count > FCCH_MAX_MISSES) {
-      start_pos = -1;
-      hit_count = miss_count = 0;
+  enum states {
+    init, search, found_something, fcch_found, search_fail
+  } fcch_search_state;
+
+  fcch_search_state = init;
+
+  while ((!end) && (i < nitems)) {
+    switch (fcch_search_state) {
+
+      case init:
+        hit_count = 0;
+        miss_count = 0;
+        start_pos = -1;
+        lowest_max_min_diff = 99999;
+        d_phase_diff_buffer.clear();
+        fcch_search_state = search;
+
+        break;
+
+      case search:
+        i++;
+
+        if (i > nitems - BUFFER_SIZE) {
+          to_consume = i;
+          fcch_search_state = search_fail;
+        }
+
+        conjprod = in[i] * conj(in[i-1]);
+
+        phase_diff = gr_fast_atan2f(imag(conjprod), real(conjprod));
+
+        if (phase_diff > 0) {
+//          start_pos = i - 1;
+          to_consume = i;
+          fcch_search_state = found_something;
+        } else {
+          fcch_search_state = search;
+        }
+
+        break;
+
+      case found_something:
+
+        if (phase_diff > 0) {
+          hit_count++;
+        } else {
+          miss_count++;
+        }
+
+        //DCOUT("d_phase_diff_buffer.size(): " << d_phase_diff_buffer.size() << " hit_count: " << hit_count);
+        
+        if ((miss_count >= FCCH_MAX_MISSES) && (hit_count <= FCCH_HITS_NEEDED)) {
+          fcch_search_state = init;
+          continue;
+        } else if ((miss_count >= FCCH_MAX_MISSES) && (hit_count > FCCH_HITS_NEEDED)) {
+          fcch_search_state = fcch_found;
+          continue;
+        } else if ((miss_count < FCCH_MAX_MISSES) && (hit_count > FCCH_HITS_NEEDED)) {
+          //find difference between minimal and maximal element in the buffer
+          //for FCCH this value should be low
+          //this part is searching for a region where this value is lowest
+          min_phase_diff = *(min_element(d_phase_diff_buffer.begin(), d_phase_diff_buffer.end()));
+          max_phase_diff = *(max_element(d_phase_diff_buffer.begin(), d_phase_diff_buffer.end()));
+          
+          if(lowest_max_min_diff > max_phase_diff - min_phase_diff){
+            lowest_max_min_diff = max_phase_diff - min_phase_diff;
+            start_pos = i - FCCH_HITS_NEEDED;
+            d_best_sum = 0;
+            for (buffer_iter = (d_phase_diff_buffer.begin());
+                 buffer_iter != (d_phase_diff_buffer.end());
+                 buffer_iter++) {
+                   d_best_sum += *buffer_iter;
+            }
+            DCOUT(d_best_sum);
+          }
+        }
+
+        i++;
+
+        if (i >= nitems) {
+          fcch_search_state = search_fail;
+          continue;
+        }
+
+        conjprod = in[i] * conj(in[i-1]);
+        phase_diff = gr_fast_atan2f(imag(conjprod), real(conjprod));
+        d_phase_diff_buffer.push_back(phase_diff);
+
+        fcch_search_state = found_something;
+
+        break;
+
+      case fcch_found:
+        DCOUT("znalezione fcch na pozycji" << d_counter + start_pos);
+        to_consume = start_pos + FCCH_HITS_NEEDED + 1;
+        /*  mean = d_best_sum / FCCH_HITS_NEEDED;
+        phase_offset = mean - (M_PI / 2);
+        freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);*/
+        compute_freq_offset();
+        end = true;
+        result = true;
+        break;
+
+      case search_fail:
+        end = true;
+        result = false;
+        break;
     }
   }
 
-  /*
-    //Do we have a match?
-    if (start_pos >= 0) {
-      //Is it within range? (we know it's long enough then too)
-      if (start_pos < 2*MAX_CORR_DIST) {
-        d_burst_start = start_pos;
-        d_bbuf_pos = 0; //load buffer from start
-        return FCCH;
-      } else {
-        //TODO: don't shift a tiny amount
-        shift_burst(start_pos - MAX_CORR_DIST);
-      }
-    } else {
-      //Didn't find anything
-      d_burst_start = MAX_CORR_DIST;
-      d_bbuf_pos = 0; //load buffer from start
-    }
-  */
-  //DCOUT("start_pos: " << start_pos);
+  d_counter += to_consume;
+  consume_each(to_consume);
+
+  return result;
+}
+
+bool find_sch_burst( const gr_complex *in, const int nitems , gr_complex *out);
+
+
+double gsm_receiver_cf::compute_freq_offset()
+{
+  float mean, phase_offset, freq_offset;
+
+  DCOUT(" d_phase_diff_buffer.size(): " <<  d_phase_diff_buffer.size());
+
+  mean = d_best_sum / FCCH_HITS_NEEDED;
+  phase_offset = mean - (M_PI / 2);
+  freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);
+  DCOUT("freq_offset: " << freq_offset);
+  d_fcch_count++;
+  d_x_temp += freq_offset;
+  d_x2_temp += freq_offset * freq_offset;
+  d_mean = d_x_temp / d_fcch_count;
+  //d_mean = 0.9 * freq_offset + 0.1 * d_mean;
+
+
+  //d_tuner->calleval(freq_offset);
+
+  d_prev_freq_offset -= freq_offset;
+
+  DCOUT("wariancja: " << sqrt((d_x2_temp / d_fcch_count - d_mean * d_mean)) << " fcch_count:" << d_fcch_count << " d_mean: " << d_mean);
 
-  return start_pos;
+  return freq_offset;
 }
 
+void gsm_receiver_cf::set_frequency()
+{
+
+}
+
+bool gsm_receiver_cf::find_sch_burst( const gr_complex *in, const int nitems , gr_complex *out)
+{
+  
+}
diff --git a/src/lib/gsm_receiver_cf.h b/src/lib/gsm_receiver_cf.h
index 5bc7617..2012325 100644
--- a/src/lib/gsm_receiver_cf.h
+++ b/src/lib/gsm_receiver_cf.h
@@ -24,8 +24,8 @@
 
 #include <gr_block.h>
 #include <gr_complex.h>
-
-#define BUFFER_SIZE 4096
+#include <gr_feval.h>
+#include <boost/circular_buffer.hpp>
 
 class gsm_receiver_cf;
 
@@ -42,6 +42,8 @@ class gsm_receiver_cf;
  */
 typedef boost::shared_ptr<gsm_receiver_cf> gsm_receiver_cf_sptr;
 
+typedef boost::circular_buffer<float> circular_buffer_float;
+
 /*!
  * \brief Return a shared_ptr to a new instance of gsm_receiver_cf.
  *
@@ -49,7 +51,7 @@ typedef boost::shared_ptr<gsm_receiver_cf> gsm_receiver_cf_sptr;
  * constructor is private.  howto_make_square_ff is the public
  * interface for creating new instances.
  */
-gsm_receiver_cf_sptr gsm_make_receiver_cf( int osr );
+gsm_receiver_cf_sptr gsm_make_receiver_cf( gr_feval_dd *tuner, int osr );
 
 /*!
  * \brief Receives fcch
@@ -61,13 +63,27 @@ class gsm_receiver_cf : public gr_block
 {
 
   private:
+    gr_feval_dd *d_tuner;
+    const int d_osr;
     int d_counter;
-    int d_return;
-    float d_phase_diff_buffer[BUFFER_SIZE];
+    float d_prev_freq_offset;
+    circular_buffer_float  d_phase_diff_buffer;
+    double d_x_temp, d_x2_temp, d_mean;
+    int d_fcch_count;
+    double d_best_sum;
     
-    friend gsm_receiver_cf_sptr gsm_make_receiver_cf( int osr );
-    gsm_receiver_cf( int osr );
-    int find_fcch_burst( const gr_complex *in, const int items );
+    enum states
+    {
+      fcch_search, sch_search
+    } d_state;
+
+    friend gsm_receiver_cf_sptr gsm_make_receiver_cf( gr_feval_dd *tuner, int osr );
+    gsm_receiver_cf( gr_feval_dd *tuner, int osr );
+
+    bool find_fcch_burst( const gr_complex *in, const int nitems );
+    bool find_sch_burst( const gr_complex *in, const int nitems , gr_complex *out);
+    double compute_freq_offset ( );
+    void set_frequency ( );
 
   public:
     ~gsm_receiver_cf();