liettle changes to fcch search, commit before test to find reason of bad behavior when OSR value is changed

3 files changed, 118 insertions, 78 deletions

diff --git a/src/lib/Assert.h b/src/lib/Assert.h
index acfb3f7..67fe02f 100644
--- a/src/lib/Assert.h
+++ b/src/lib/Assert.h
@@ -26,7 +26,7 @@
 #include "stdio.h"
 #include <iostream>
 
-#define NDEBUG
+//#define NDEBUG
 
 /**@name Macros for standard messages. */
 //@{
diff --git a/src/lib/gsm_receiver_cf.cc b/src/lib/gsm_receiver_cf.cc
index 5f730dc..045c639 100644
--- a/src/lib/gsm_receiver_cf.cc
+++ b/src/lib/gsm_receiver_cf.cc
@@ -29,10 +29,17 @@
 #include <gr_math.h>
 #include <math.h>
 #include <Assert.h>
+#include <list>
+#include <boost/circular_buffer.hpp>
 #include <algorithm>
 
-#define SAFETY_MARGIN 50
-#define BUFFER_SIZE (FCCH_HITS_NEEDED)
+#define FCCH_BUFFER_SIZE (FCCH_HITS_NEEDED)
+#define SYNC_SEARCH_RANGE 40
+
+typedef std::list<float> list_float;
+typedef std::vector<float> vector_float;
+
+typedef boost::circular_buffer<float> circular_buffer_float;
 
 gsm_receiver_cf_sptr
 gsm_make_receiver_cf(gr_feval_dd *tuner, int osr)
@@ -58,10 +65,9 @@ gsm_receiver_cf::gsm_receiver_cf(gr_feval_dd *tuner, int osr)
     d_fcch_start_pos(0),
     d_freq_offset(0),
     d_state(first_fcch_search),
-    d_first(false)
-    //    d_fcch_count(0),
-    //    d_x_temp(0),
-    //    d_x2_temp(0)
+    d_fcch_count(0), //!!
+    d_x_temp(0),//!!
+    d_x2_temp(0)//!!
 {
   gmsk_mapper(SYNC_BITS, d_sch_training_seq, N_SYNC_BITS);
 }
@@ -75,7 +81,7 @@ gsm_receiver_cf::~gsm_receiver_cf()
 
 void gsm_receiver_cf::forecast(int noutput_items, gr_vector_int &ninput_items_required)
 {
-  ninput_items_required[0] = noutput_items * (TS_BITS + SAFETY_MARGIN) * d_OSR;
+  ninput_items_required[0] = noutput_items * (TS_BITS + 2 * SAFETY_MARGIN) * d_OSR;
 }
 
 int
@@ -87,11 +93,10 @@ 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 = 0;
+  float prev_freq_offset;
 
   switch (d_state) {
-
     case first_fcch_search:
-
       if (find_fcch_burst(in, ninput_items[0])) {
         set_frequency(d_freq_offset);
         produced_out = 0;
@@ -100,11 +105,15 @@ gsm_receiver_cf::general_work(int noutput_items,
         produced_out = 0;
         d_state = first_fcch_search;
       }
-
       break;
 
     case next_fcch_search:
+      prev_freq_offset = d_freq_offset;
       if (find_fcch_burst(in, ninput_items[0])) {
+        if (abs(d_freq_offset) > 100) {
+          float mean_freq_offset = (prev_freq_offset + d_freq_offset) / 2;
+//           set_frequency(d_freq_offset);
+        }
         produced_out = 0;
         d_state = sch_search;
       } else {
@@ -115,12 +124,13 @@ gsm_receiver_cf::general_work(int noutput_items,
 
     case sch_search:
       if (find_sch_burst(in, ninput_items[0], out)) {
-        d_state = read_bcch;
+//         d_state = read_bcch;
+        d_state = next_fcch_search;
       } else {
         d_state = sch_search;
       }
       break;
-      
+
     case read_bcch:
       break;
   }
@@ -130,7 +140,7 @@ gsm_receiver_cf::general_work(int noutput_items,
 
 bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
 {
-  circular_buffer_float  phase_diff_buffer(BUFFER_SIZE * d_OSR);
+  circular_buffer_float phase_diff_buffer(FCCH_BUFFER_SIZE * d_OSR);
 
   float phase_diff = 0;
   gr_complex conjprod;
@@ -145,6 +155,7 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
   int sample_number = 0;
   bool end = false;
   bool result = false;
+//   float mean=0, phase_offset=0, freq_offset=0;
 
   circular_buffer_float::iterator buffer_iter;
 
@@ -170,11 +181,10 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
       case search:
         sample_number++;
 
-        if (sample_number > nitems - BUFFER_SIZE * d_OSR) {
+        if (sample_number > nitems - FCCH_BUFFER_SIZE * d_OSR) {
           to_consume = sample_number;
           fcch_search_state = search_fail;
         } else {
-
           phase_diff = compute_phase_diff(in[sample_number], in[sample_number-1]);
 
           if (phase_diff > 0) {
@@ -188,7 +198,6 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
         break;
 
       case found_something:
-
         if (phase_diff > 0) {
           hit_count++;
         } else {
@@ -197,7 +206,7 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
 
         if ((miss_count >= FCCH_MAX_MISSES * d_OSR) && (hit_count <= FCCH_HITS_NEEDED * d_OSR)) {
           fcch_search_state = init;
-          //  DCOUT("hit_count: " << hit_count << " miss_count: " << miss_count << " d_counter: " << d_counter);
+          //DCOUT("hit_count: " << hit_count << " miss_count: " << miss_count << " d_counter: " << d_counter);
           continue;
         } else if ((miss_count >= FCCH_MAX_MISSES * d_OSR) && (hit_count > FCCH_HITS_NEEDED * d_OSR)) {
           fcch_search_state = fcch_found;
@@ -214,7 +223,7 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
             start_pos = sample_number - FCCH_HITS_NEEDED * d_OSR - FCCH_MAX_MISSES * d_OSR;
             d_best_sum = 0;
 
-            for (buffer_iter = (phase_diff_buffer.begin());
+            for (buffer_iter = phase_diff_buffer.begin();
                  buffer_iter != (phase_diff_buffer.end());
                  buffer_iter++) {
               d_best_sum += *buffer_iter - (M_PI / 2) / d_OSR;
@@ -231,7 +240,6 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
 
         phase_diff = compute_phase_diff(in[sample_number], in[sample_number-1]);
         phase_diff_buffer.push_back(phase_diff);
-
         fcch_search_state = found_something;
 
         break;
@@ -239,9 +247,9 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
       case fcch_found:
         DCOUT("znalezione fcch na pozycji" << d_counter + start_pos);
         to_consume = start_pos + FCCH_HITS_NEEDED * d_OSR + 1;
-        /*  mean = d_best_sum / FCCH_HITS_NEEDED;
-        phase_offset = mean - (M_PI / 2);
-        freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);*/
+//         mean = d_best_sum / FCCH_HITS_NEEDED;
+//         phase_offset = mean - (M_PI / 2);
+//         freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);
         d_fcch_start_pos = d_counter + start_pos;
         compute_freq_offset();
         end = true;
@@ -255,14 +263,7 @@ bool gsm_receiver_cf::find_fcch_burst(const gr_complex *in, const int nitems)
     }
   }
 
-  //  DCOUT("hit_count: " << hit_count << " miss_count: " << miss_count << " d_counter: " << d_counter);
-        //!
-  for(int i = 0; i < to_consume; i++){
-    //std::cout << "(" << real(in[i]) << "," << imag(in[i]) << ")\n";
-  }
-  
   d_counter += to_consume;
-
   consume_each(to_consume);
 
   return result;
@@ -277,12 +278,13 @@ double gsm_receiver_cf::compute_freq_offset()
   freq_offset = phase_offset * 1625000.0 / (12.0 * M_PI);
   d_freq_offset -= freq_offset;
 
-//  d_x_temp += freq_offset;
-//  d_x2_temp += freq_offset * freq_offset;
-//  d_mean = d_x_temp / d_fcch_count;
-//   d_fcch_count++;
-   DCOUT("freq_offset: " << freq_offset << " d_best_sum: " << d_best_sum);
-//   DCOUT("wariancja: " << sqrt((d_x2_temp / d_fcch_count - d_mean * d_mean)) << " fcch_count:" << d_fcch_count << " d_mean: " << d_mean);
+  d_fcch_count++;
+  d_x_temp += freq_offset;
+  d_x2_temp += freq_offset * freq_offset;
+  d_mean = d_x_temp / d_fcch_count;
+
+  DCOUT("freq_offset: " << freq_offset );//" d_best_sum: " << d_best_sum
+  DCOUT("wariancja: " << sqrt((d_x2_temp / d_fcch_count - d_mean * d_mean)) << " fcch_count:" << d_fcch_count << " d_mean: " << d_mean);
 
   return freq_offset;
 }
@@ -299,19 +301,27 @@ bool gsm_receiver_cf::find_sch_burst(const gr_complex *in, const int nitems , fl
   bool end = false;
   bool result = false;
   int sample_nr_near_sch_start = d_fcch_start_pos + (FRAME_BITS - SAFETY_MARGIN) * d_OSR;
+  vector_complex correlation_buffer;
+  list_float power_buffer;
+  vector_float window_energy_buffer;
+  int strongest_window_nr;
 
-  
   enum states {
     start, reach_sch, find_sch_start, search_not_finished, sch_found
   } sch_search_state;
-  
+
   sch_search_state = start;
+  //!!!!
+  int chan_imp_length = 4;
+  float energy = 0;
+  bool loop_end = false;
+  list_float::iterator iter;
 
-  
   while (!end) {
     switch (sch_search_state) {
+
       case start:
-        if(d_counter < sample_nr_near_sch_start){
+        if (d_counter < sample_nr_near_sch_start) {
           sch_search_state = reach_sch;
         } else {
           sch_search_state = find_sch_start;
@@ -319,48 +329,67 @@ bool gsm_receiver_cf::find_sch_burst(const gr_complex *in, const int nitems , fl
         break;
 
       case reach_sch:
-
-        if(d_counter + nitems >= sample_nr_near_sch_start){
+        if (d_counter + nitems >= sample_nr_near_sch_start) {
           to_consume = sample_nr_near_sch_start - d_counter;
-          DCOUT("reach_sch consumes: " << to_consume << "bits");
         } else {
           to_consume = nitems;
         }
-        
-        
-
         sch_search_state = search_not_finished;
         break;
 
       case find_sch_start:
-        DCOUT("find_sch_start nitems" << nitems);
-//         if ((sch_start >= d_counter) && (sch_start <= d_counter + nitems)) {
-//           for (int i = 0; i < ninput_items[0] - N_SYNC_BITS; i++) {
-//             gr_complex result;
-//             d_counter++;
-//             result =  correlation(&d_sch_training_seq[5], in + i, N_SYNC_BITS - 10);
-// 
-//             if (abs(result) > 60000) {
-//               DCOUT("znaleziono środek sch na pozycji: " << d_counter);
-//             }
-// 
-// //          std::cout << "(" << real(in[i]) << "," << imag(in[i]) << ")\n";
-// //           std::cout << "(" << real(result) << "," << imag(result) << ")\n";
-// 
-//             int ninput = N_SYNC_BITS - 10;
-//             const gr_complex * input_signal = in + i;
-//             gr_complex * sequence = &d_sch_training_seq[5];
-//           }
-//         }
-        to_consume = nitems;
+//        DCOUT("find_sch_start d_counter" << d_counter);
+        for (int ii = SYNC_POS * d_OSR; ii < (SYNC_POS + SYNC_SEARCH_RANGE)*d_OSR; ii++) {
+          to_consume++;
+          gr_complex correlation =  correlate_sequence(&d_sch_training_seq[5], &in[ii], N_SYNC_BITS - 10);
+          correlation_buffer.push_back(correlation); // tylko do znalezienia odp imp kanału
+          power_buffer.push_back(pow(abs(correlation), 2));
+          if (abs(correlation) > 30000) {
+            DCOUT("znaleziono środek sch na pozycji: " << ii - SYNC_POS * d_OSR);
+          }
+        }
+
+        //compute window energies
+        iter = power_buffer.begin();
+        while (iter != power_buffer.end()) {
+          list_float::iterator iter_ii = iter;
+          energy = 0;
+
+          for (int ii = 0; ii < chan_imp_length; ii++, iter_ii++) {
+            if (iter_ii == power_buffer.end()) {
+              loop_end = true;
+              break;
+            }
+            energy += (*iter_ii);
+          }
+
+          if (loop_end) {
+            break;
+          }
+          iter++;
+//          std::cout << energy << "\n";
+          window_energy_buffer.push_back(energy);
+        }
+
+        strongest_window_nr = max_element(window_energy_buffer.begin(), window_energy_buffer.end()) - window_energy_buffer.begin();
+        d_channel_imp_resp.clear();
+        for (int ii = 0; ii < chan_imp_length; ii++) {
+          gr_complex correlation = correlation_buffer[strongest_window_nr + (ii * d_OSR)];
+          d_channel_imp_resp.push_back(correlation);
+        }
+
+        DCOUT("strongest_window_nr: " << strongest_window_nr);
+
         sch_search_state = sch_found;
         break;
 
       case search_not_finished:
+        result = false;
         end = true;
         break;
 
       case sch_found:
+        result = true;
         end = true;
         break;
     }
@@ -393,12 +422,12 @@ void gsm_receiver_cf::gmsk_mapper(const int * input, gr_complex * output, int ni
   }
 }
 
-gr_complex gsm_receiver_cf::correlation(const gr_complex * sequence, const gr_complex * input_signal, int ninput)
+gr_complex gsm_receiver_cf::correlate_sequence(const gr_complex * sequence, const gr_complex * input_signal, int length)
 {
   gr_complex result(0.0, 0.0);
   int sample_number = 0;
 
-  for (int ii = 0; ii < ninput; ii++) {
+  for (int ii = 0; ii < length; ii++) {
     sample_number = (ii * d_OSR) ;
     result += sequence[ii] * conj(input_signal[sample_number]);
   }
@@ -406,6 +435,19 @@ gr_complex gsm_receiver_cf::correlation(const gr_complex * sequence, const gr_co
   return result;
 }
 
+// gr_complex gsm_receiver_cf::compute_energy(const gr_complex * input_signal, int length)
+// {
+//   float result = 0;
+//   int sample_number = 0;
+//
+//   for (int ii = 0; ii < length; ii++) {
+//     result += input_signal[(ii * d_OSR)];
+//   }
+//
+//   return result;
+// }
+
+
 // gr_complex gsm_receiver_cf::calc_energy(int window_len){
 //
 // }
@@ -413,4 +455,4 @@ inline float gsm_receiver_cf::compute_phase_diff(gr_complex val1, gr_complex val
 {
   gr_complex conjprod = val1 * conj(val2);
   return gr_fast_atan2f(imag(conjprod), real(conjprod));
-}
\ No newline at end of file
+}
diff --git a/src/lib/gsm_receiver_cf.h b/src/lib/gsm_receiver_cf.h
index ccc99fb..d4e26e5 100644
--- a/src/lib/gsm_receiver_cf.h
+++ b/src/lib/gsm_receiver_cf.h
@@ -25,8 +25,8 @@
 #include <gr_block.h>
 #include <gr_complex.h>
 #include <gr_feval.h>
-#include <boost/circular_buffer.hpp>
 #include <gsm_constants.h>
+#include <vector>
 
 
 class gsm_receiver_cf;
@@ -43,8 +43,7 @@ class gsm_receiver_cf;
  * As a convention, the _sptr suffix indicates a boost::shared_ptr
  */
 typedef boost::shared_ptr<gsm_receiver_cf> gsm_receiver_cf_sptr;
-
-typedef boost::circular_buffer<float> circular_buffer_float;
+typedef std::vector<gr_complex> vector_complex;
 
 /*!
  * \brief Return a shared_ptr to a new instance of gsm_receiver_cf.
@@ -75,12 +74,11 @@ class gsm_receiver_cf : public gr_block
     int d_fcch_start_pos;
     float d_freq_offset;
     double d_best_sum;
-
-    int d_fcch_count;
     
-    bool d_first;
-//    int d_fcch_count;
-//    double d_x_temp, d_x2_temp, d_mean;
+    int d_fcch_count; //!!!
+    double d_x_temp, d_x2_temp, d_mean;//!!
+    
+    vector_complex d_channel_imp_resp;
 
     enum states {
       first_fcch_search, next_fcch_search, sch_search, read_bcch
@@ -96,7 +94,7 @@ class gsm_receiver_cf : public gr_block
     
     bool find_sch_burst(const gr_complex *in, const int nitems , float *out);
     void gmsk_mapper(const int * input, gr_complex * gmsk_output, int ninput);
-    gr_complex correlation(const gr_complex * sequence, const gr_complex * input_signal, int ninput);
+    gr_complex correlate_sequence(const gr_complex * sequence, const gr_complex * input_signal, int ninput);
     
 
   public: