Corrected precision of FCCH search function

5 files changed, 450 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();
diff --git a/src/python/cfile b/src/python/cfile
new file mode 100644
index 0000000..943e35f
--- /dev/null
+++ b/src/python/cfile
diff --git a/src/python/gsm_findfcch.py b/src/python/gsm_findfcch.py
new file mode 100755
index 0000000..7ab9c18
--- /dev/null
+++ b/src/python/gsm_findfcch.py
@@ -0,0 +1,108 @@
+#!/usr/bin/env python
+#!/usr/bin/env python
+
+from gnuradio import gr, gru, blks2
+#, gsm
+from gnuradio.eng_option import eng_option
+from optparse import OptionParser
+from os import sys
+
+for extdir in ['../../debug/src/lib','../../debug/src/lib/.libs']:
+    if extdir not in sys.path:
+        sys.path.append(extdir)
+import gsm                        
+
+class tune(gr.feval_dd):
+    def __init__(self, top_block):
+        gr.feval_dd.__init__(self)
+        self.top_block = top_block
+        self.center_freq = 0
+    def eval(self, freq_offet):
+        self.center_freq = self.center_freq - freq_offet
+        self.top_block.set_frequency(self.center_freq)
+        return self.center_freq
+
+
+class gsm_receiver_first_blood(gr.top_block):
+    def __init__(self):
+        gr.top_block.__init__(self)
+        (options, args) = self._przetworz_opcje()
+        self.tune_callback = tune(self)
+        self.options    = options
+        self.args       = args
+        self._ustaw_taktowanie()
+        self.zrodlo = self._ustaw_zrodlo()
+        self.filtr = self._ustaw_filtr()
+        self.interpolator = self._ustaw_interpolator()
+        self.odbiornik = self._ustaw_odbiornik()
+        self.konwerter = self._ustaw_konwerter()
+        self.ujscie = self._ustaw_ujscie()
+    
+        self.connect(self.zrodlo, self.filtr,  self.interpolator, self.odbiornik, self.konwerter, self.ujscie)
+#       self.connect(self.zrodlo, self.ujscie)
+  
+    def _ustaw_ujscie(self):
+        nazwa_pliku_wy = self.options.outputfile
+        ujscie = gr.file_sink(gr.sizeof_float, nazwa_pliku_wy)
+        return ujscie
+    
+    def _ustaw_zrodlo(self):
+        nazwa_pliku = self.options.inputfile
+        zrodlo = gr.file_source(gr.sizeof_gr_complex, nazwa_pliku, False)
+        return zrodlo
+    
+    def _ustaw_taktowanie(self):
+        options = self.options
+        clock_rate = 64e6
+        self.clock_rate = clock_rate
+        self.input_rate = clock_rate / options.decim
+        self.gsm_symb_rate = 1625000.0 / 6.0
+        self.sps = self.input_rate / self.gsm_symb_rate
+
+    def _ustaw_filtr(self):
+        filter_cutoff   = 145e3	
+        filter_t_width  = 10e3
+        offset = 0
+        print "input_rate:", self.input_rate, "sample rate:", self.sps, " filter_cutoff:", filter_cutoff, " filter_t_width:", filter_t_width
+        filter_taps     = gr.firdes.low_pass(1.0, self.input_rate, filter_cutoff, filter_t_width, gr.firdes.WIN_HAMMING)
+        filtr          = gr.freq_xlating_fir_filter_ccf(1, filter_taps, offset, self.input_rate)
+        return filtr
+
+    def _ustaw_konwerter(self):
+        v2s = gr.vector_to_stream(gr.sizeof_float, 142)
+        return v2s
+    
+    def _ustaw_interpolator(self):
+        interpolator = gr.fractional_interpolator_cc(0, self.sps) 
+        return interpolator
+    
+    def _ustaw_odbiornik(self):
+        odbiornik = gsm.receiver_cf(self.tune_callback, self.options.osr)
+        return odbiornik
+    
+    def _przetworz_opcje(self):
+        parser = OptionParser(option_class=eng_option)
+        parser.add_option("-d", "--decim", type="int", default=128,
+                          help="Set USRP decimation rate to DECIM [default=%default]")
+        parser.add_option("-r", "--osr", type="int", default=1,
+                          help="Oversampling ratio [default=%default]")
+        parser.add_option("-I", "--inputfile", type="string", default="cfile",
+                          help="Input filename")
+        parser.add_option("-O", "--outputfile", type="string", default="cfile2.out",
+                          help="Output filename")
+        (options, args) = parser.parse_args ()
+        return (options, args)
+    
+    def set_frequency(self, center_freq):
+        self.filtr.set_center_freq(center_freq)
+
+def main():
+    try:
+        gsm_receiver_first_blood().run()
+    except KeyboardInterrupt:
+        pass
+
+if __name__ == '__main__':
+    main()
+
+
diff --git a/src/python/gsm_findfcch_usrp.py b/src/python/gsm_findfcch_usrp.py
new file mode 100755
index 0000000..7bf81f3
--- /dev/null
+++ b/src/python/gsm_findfcch_usrp.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python
+#!/usr/bin/env python
+
+from gnuradio import gr, gru, blks2
+from gnuradio import usrp
+#from gnuradio import gsm
+from gnuradio.eng_option import eng_option
+from optparse import OptionParser
+from os import sys
+#"""
+for extdir in ['../../debug/src/lib','../../debug/src/lib/.libs']:
+    if extdir not in sys.path:
+        sys.path.append(extdir)
+import gsm                        
+#"""
+def pick_subdevice(u):
+    if u.db[0][0].dbid() >= 0:
+        return (0, 0)
+    if u.db[1][0].dbid() >= 0:
+        return (1, 0)
+    return (0, 0)
+
+class gsm_receiver_first_blood(gr.top_block):
+    def __init__(self):
+        gr.top_block.__init__(self)
+        ( self.options, self.args) = self._przetworz_opcje()
+        self._ustaw_taktowanie()
+        self.zrodlo = self._ustaw_zrodlo()
+        self.filtr = self._ustaw_filtr()
+        self.interpolator = self._ustaw_interpolator()
+        self.odbiornik = self._ustaw_odbiornik()
+        self.konwerter = self._ustaw_konwerter()
+        self.ujscie = self._ustaw_ujscie()
+    
+        self.connect(self.zrodlo, self.filtr,  self.interpolator, self.odbiornik, self.konwerter, self.ujscie)
+#       self.connect(self.zrodlo, self.ujscie)
+  
+    def _ustaw_ujscie(self):
+        nazwa_pliku_wy = self.options.outputfile
+        ujscie = gr.file_sink(gr.sizeof_float, nazwa_pliku_wy)
+        return ujscie
+    
+    def _ustaw_zrodlo(self):	
+        options = self.options
+        fusb_block_size = gr.prefs().get_long('fusb', 'block_size', 4096)
+        fusb_nblocks    = gr.prefs().get_long('fusb', 'nblocks', 16)
+        self.usrp = usrp.source_c(decim_rate=options.decim, fusb_block_size=fusb_block_size, fusb_nblocks=fusb_nblocks)
+        
+        if options.rx_subdev_spec is None:
+            options.rx_subdev_spec = pick_subdevice(self.usrp)
+        
+        self.usrp.set_mux(usrp.determine_rx_mux_value(self.usrp, options.rx_subdev_spec))
+        # determine the daughterboard subdevice
+        self.subdev = usrp.selected_subdev(self.usrp, options.rx_subdev_spec)
+        input_rate = self.usrp.adc_freq() / self.usrp.decim_rate()
+
+        # set initial values
+        if options.gain is None:
+            # if no gain was specified, use the mid-point in dB
+            g = self.subdev.gain_range()
+            options.gain = float(g[0]+g[1])/2
+
+        r = self.usrp.tune(0, self.subdev, options.freq)
+        self.subdev.set_gain(options.gain)
+        return self.usrp
+    
+    def _ustaw_taktowanie(self):
+        options = self.options
+        clock_rate = 64e6
+        self.clock_rate = clock_rate
+        self.input_rate = clock_rate / options.decim
+        self.gsm_symb_rate = 1625000.0 / 6.0
+        self.sps = self.input_rate / self.gsm_symb_rate
+
+    def _ustaw_filtr(self):
+        filter_cutoff   = 145e3	
+        filter_t_width  = 10e3
+        offset = 0
+        print "input_rate:", self.input_rate, "sample rate:", self.sps, " filter_cutoff:", filter_cutoff, " filter_t_width:", filter_t_width
+        filter_taps     = gr.firdes.low_pass(1.0, self.input_rate, filter_cutoff, filter_t_width, gr.firdes.WIN_HAMMING)
+        filtr          = gr.freq_xlating_fir_filter_ccf(1, filter_taps, offset, self.input_rate)
+        return filtr
+
+    def _ustaw_konwerter(self):
+        v2s = gr.vector_to_stream(gr.sizeof_float, 142)
+        return v2s
+    
+    def _ustaw_interpolator(self):
+        interpolator = gr.fractional_interpolator_cc(0, self.sps) 
+        return interpolator
+    
+    def _ustaw_odbiornik(self):
+        odbiornik = gsm.receiver_cf(1)
+        return odbiornik
+    
+    def _przetworz_opcje(self):
+        parser = OptionParser(option_class=eng_option)
+        parser.add_option("-d", "--decim", type="int", default=128,
+                                    help="Set USRP decimation rate to DECIM [default=%default]")
+        parser.add_option("-I", "--inputfile", type="string", default="cfile",
+                                    help="Input filename")
+        parser.add_option("-O", "--outputfile", type="string", default="cfile2.out",
+                                    help="Output filename")
+        parser.add_option("-R", "--rx-subdev-spec", type="subdev", default=None,
+                                    help="Select USRP Rx side A or B (default=first one with a daughterboard)")   
+        parser.add_option("-f", "--freq", type="eng_float", default="950.4M",
+                                    help="set frequency to FREQ", metavar="FREQ")
+        parser.add_option("-g", "--gain", type="eng_float", default=None,
+                                    help="Set gain in dB (default is midpoint)")
+        (options, args) = parser.parse_args ()
+        return (options, args)
+
+def main():
+    try:
+        gsm_receiver_first_blood().run()
+    except KeyboardInterrupt:
+        pass
+
+if __name__ == '__main__':
+    main()
+
+