adaptive DFE and added frequency offset tracking added

CMakeLists.txt

@@ -22,6 +22,7 @@
 # Project setup
 ########################################################################
 cmake_minimum_required(VERSION 2.6)
+set(CMAKE_CXX_STANDARD 11)
 project(gr-digitalhf CXX C)
 enable_testing()
 
@@ -148,7 +149,7 @@ find_package(Doxygen)
 # components required to the list of GR_REQUIRED_COMPONENTS (in all
 # caps such as FILTER or FFT) and change the version to the minimum
 # API compatible version required.
-set(GR_REQUIRED_COMPONENTS RUNTIME DIGITAL)
+set(GR_REQUIRED_COMPONENTS RUNTIME DIGITAL VOLK)
 find_package(Gnuradio "3.7.2" REQUIRED)
 list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake/Modules)
 include(GrVersion)

examples/1st.grc

@@ -117,7 +117,7 @@
     </param>
     <param>
       <key>value</key>
-      <value>10*sps</value>
+      <value>12*sps</value>
     </param>
   </block>
   <block>
@@ -144,7 +144,7 @@
     </param>
     <param>
       <key>value</key>
-      <value>5*sps</value>
+      <value>12*sps</value>
     </param>
   </block>
   <block>
@@ -395,6 +395,14 @@
       <key>id</key>
       <value>digitalhf_adaptive_dfe_0</value>
     </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
     <param>
       <key>sps</key>
       <value>sps</value>
@@ -416,6 +424,373 @@
       <value>STANAG_4285</value>
     </param>
   </block>
+  <block>
+    <key>qtgui_time_sink_x</key>
+    <param>
+      <key>autoscale</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>axislabels</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>comment</key>
+      <value></value>
+    </param>
+    <param>
+      <key>ctrlpanel</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>entags</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(874, 176)</value>
+    </param>
+    <param>
+      <key>gui_hint</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>grid</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>id</key>
+      <value>qtgui_time_sink_x_0</value>
+    </param>
+    <param>
+      <key>legend</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>alpha1</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color1</key>
+      <value>"blue"</value>
+    </param>
+    <param>
+      <key>label1</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker1</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style1</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width1</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha10</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color10</key>
+      <value>"blue"</value>
+    </param>
+    <param>
+      <key>label10</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker10</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style10</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width10</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha2</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color2</key>
+      <value>"red"</value>
+    </param>
+    <param>
+      <key>label2</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker2</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style2</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width2</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha3</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color3</key>
+      <value>"green"</value>
+    </param>
+    <param>
+      <key>label3</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker3</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style3</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width3</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha4</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color4</key>
+      <value>"black"</value>
+    </param>
+    <param>
+      <key>label4</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker4</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style4</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width4</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha5</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color5</key>
+      <value>"cyan"</value>
+    </param>
+    <param>
+      <key>label5</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker5</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style5</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width5</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha6</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color6</key>
+      <value>"magenta"</value>
+    </param>
+    <param>
+      <key>label6</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker6</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style6</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width6</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha7</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color7</key>
+      <value>"yellow"</value>
+    </param>
+    <param>
+      <key>label7</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker7</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style7</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width7</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha8</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color8</key>
+      <value>"dark red"</value>
+    </param>
+    <param>
+      <key>label8</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker8</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style8</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width8</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alpha9</key>
+      <value>1.0</value>
+    </param>
+    <param>
+      <key>color9</key>
+      <value>"dark green"</value>
+    </param>
+    <param>
+      <key>label9</key>
+      <value></value>
+    </param>
+    <param>
+      <key>marker9</key>
+      <value>-1</value>
+    </param>
+    <param>
+      <key>style9</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>width9</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>name</key>
+      <value>""</value>
+    </param>
+    <param>
+      <key>nconnections</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>size</key>
+      <value>1024</value>
+    </param>
+    <param>
+      <key>srate</key>
+      <value>samp_rate</value>
+    </param>
+    <param>
+      <key>stemplot</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>tr_chan</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>tr_delay</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>tr_level</key>
+      <value>0.0</value>
+    </param>
+    <param>
+      <key>tr_mode</key>
+      <value>qtgui.TRIG_MODE_FREE</value>
+    </param>
+    <param>
+      <key>tr_slope</key>
+      <value>qtgui.TRIG_SLOPE_POS</value>
+    </param>
+    <param>
+      <key>tr_tag</key>
+      <value>""</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>complex</value>
+    </param>
+    <param>
+      <key>update_time</key>
+      <value>0.10</value>
+    </param>
+    <param>
+      <key>ylabel</key>
+      <value>Amplitude</value>
+    </param>
+    <param>
+      <key>yunit</key>
+      <value>""</value>
+    </param>
+    <param>
+      <key>ymax</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>ymin</key>
+      <value>-1</value>
+    </param>
+  </block>
   <connection>
     <source_block_id>analog_noise_source_x_0</source_block_id>
     <sink_block_id>blocks_throttle_0</sink_block_id>
@@ -428,4 +803,10 @@
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
+  <connection>
+    <source_block_id>digitalhf_adaptive_dfe_0</source_block_id>
+    <sink_block_id>qtgui_time_sink_x_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
 </flow_graph>

grc/digitalhf_adaptive_dfe.xml

@@ -51,8 +51,9 @@
        * type
        * vlen
        * optional (set to 1 for optional inputs) -->
-  <!-- <source> -->
+  <source>
-  <!--   <name>out</name> -->
+    <name>out</name>
-  <!--   <type><!-\- e.g. int, float, complex, byte, short, xxx_vector, ...-\-></type> -->
+    <type>complex</type>
-  <!-- </source> -->
+     <!-- byte, short, xxx_vector -->
+  </source>
 </block>

lib/adaptive_dfe_impl.cc

@@ -23,6 +23,7 @@
 #endif
 
 #include <gnuradio/io_signature.h>
+#include <volk/volk.h>
 #include "adaptive_dfe_impl.h"
 
 namespace gr {
@@ -31,7 +32,7 @@ namespace digitalhf {
 namespace {
 class GILLock {
   PyGILState_STATE _state;
-  public:
+public:
   GILLock()
     :_state(PyGILState_Ensure()) {}
   ~GILLock() {
@@ -39,6 +40,7 @@ class GILLock {
   }
 } ;
 }
+
 adaptive_dfe::sptr
 adaptive_dfe::make(int sps, // samples per symbol
                    int nB,  // number of forward FIR taps
@@ -60,29 +62,35 @@ adaptive_dfe_impl::adaptive_dfe_impl(int sps, // samples per symbol
                                      std::string python_module_name)
   : gr::block("adaptive_dfe",
               gr::io_signature::make(1, 1, sizeof(gr_complex)),
-              gr::io_signature::make(0, 0, sizeof(gr_complex)))
+              gr::io_signature::make(1, 1, sizeof(gr_complex)))
   , _sps(sps)
   , _nB(nB)
   , _nF(nF)
   , _nW(nW)
+  , _mu(0.01)
+  , _alpha(0.0005)
   , _py_module_name(python_module_name)
   , _physicalLayer()
-  , _taps_samples(nB+nF+1)
+  , _taps_samples(nullptr)
-  , _taps_symbols(nW)
+  , _taps_symbols(nullptr)
+  , _hist_samples(nullptr)
+  , _hist_symbols(nullptr)
+  , _hist_sample_index(0)
+  , _hist_symbol_index(0)
+  , _sample_counter(0)
   , _constellations()
   , _constellation_index()
   , _symbols()
   , _scramble()
+  , _descrambled_symbols()
+  , _symbol_counter(0)
+  , _sum_phase_diff(0)
+  , _df(0)
+  , _phase(0)
+  , _b{0.338187046465954, -0.288839024460507}
+  , _ud(0)
+  , _state(WAIT_FOR_PREAMBLE)
 {
-  // make sure python is ready for threading
-  if( Py_IsInitialized() ){
-    if(PyEval_ThreadsInitialized() != 1 ){
-      PyEval_InitThreads();
-    }
-    boost::python::numpy::initialize();
-  } else {
-    throw std::runtime_error("dont use es_pyhandler without python!");
-  }
 }
 
 /*
@@ -95,7 +103,7 @@ adaptive_dfe_impl::~adaptive_dfe_impl()
 void
 adaptive_dfe_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required)
 {
-  /* <+forecast+> e.g. ninput_items_required[0] = noutput_items */
+  ninput_items_required[0] = _sps*noutput_items;
 }
 
 int
@@ -104,21 +112,177 @@ adaptive_dfe_impl::general_work(int noutput_items,
                                 gr_vector_const_void_star &input_items,
                                 gr_vector_void_star &output_items)
 {
+  gr::thread::scoped_lock lock(d_setlock);
   gr_complex const* in = (gr_complex const *)input_items[0];
+  gr_complex *out = (gr_complex *)output_items[0];
 
-  GILLock lock;
+  int nout = 0;
-  // TODO: wait for preamble correlation tag etc...
+  int i = 0;
-  update_frame_information(_physicalLayer.attr("get_frame")());
+  for (; i<ninput_items[0] && nout < noutput_items; ++i) {
-  update_doppler_information(_physicalLayer.attr("get_doppler")()); // symbols
+    assert(nout < noutput_items);
+    _phase += _df;
+    if (_phase > M_PI)
+      _phase -= 2*M_PI;
+    if (_phase < -M_PI)
+      _phase += 2*M_PI;
 
-  consume_each (noutput_items);
+    _hist_samples[_hist_sample_index] = _hist_samples[_hist_sample_index+_nB+_nF+1] = in[i] * std::exp(gr_complex(0,_phase));
+    if (++_hist_sample_index == _nB+_nF+1)
+      _hist_sample_index = 0;
+
+    if (_state == WAIT_FOR_PREAMBLE) {
+      std::vector<tag_t> v;
+      get_tags_in_window(v, 0, i,i+1);
+      float phase_est = 0;
+      float  corr_est = 0;
+      uint64_t offset = 0;
+      for (int j=0; j<v.size(); ++j) {
+        std::cout << "tag " << v[j].key << " " << v[j].offset-nitems_read(0) << std::endl;
+        if (v[j].key == pmt::mp("phase_est")) {
+          phase_est = pmt::to_double(v[j].value);
+          std::cout << "phase_est " << v[j].offset <<" " << nitems_read(0) << " " << phase_est << std::endl;
+        }
+        if (v[j].key == pmt::mp("corr_est")) {
+          corr_est = pmt::to_double(v[j].value);
+          std::cout << "corr_est " << v[j].offset << " " << nitems_read(0) << " "
+                    << pmt::is_number(v[j].value) << " "
+                    << pmt::is_integer(v[j].value) << " "
+                    << pmt::is_real(v[j].value) << " "
+                    << pmt::to_double(v[j].value)
+                    << std::endl;
+          if (corr_est > 130e3) {
+            offset = v[j].offset - nitems_read(0);
+            break;
+          }
+        }
+      }
+      if (corr_est > 130e3) {
+        _state = DO_FILTER;
+        _sample_counter = 0;
+        _symbol_counter = 0;
+        // _symbols.clear();
+        // _scramble.clear();
+        _descrambled_symbols.clear();
+        // _hist_sample_index = 0;
+        _hist_symbol_index = 0;
+        std::fill_n(_hist_symbols, 2*_nW, gr_complex(0));
+        std::fill_n(_taps_samples, _nB+_nF+1, gr_complex(0));
+        std::fill_n(_taps_symbols, _nW, gr_complex(0));
+        //_phase = -phase_est;
+        _taps_samples[_nB+1] = std::exp(gr_complex(0, -phase_est));
+        _taps_symbols[0] = 1;
+        GILLock lock;
+        try {
+          update_frame_information(_physicalLayer.attr("get_frame")());
+        } catch (boost::python::error_already_set const&) {
+          PyErr_Print();
+        }
+      }
+    }
+    if (_state == DO_FILTER) {
+      gr_complex dot_samples = 0;
+      // volk_32fc_x2_dot_prod_32fc(&dot_samples,
+      //                            &_hist_samples.front()+_hist_sample_index,
+      //                            &_taps_samples.front(),
+      //                            _taps_samples.size());
+      gr_complex filter_output = dot_samples;
+      // if (_sample_counter < 80*5)
+      //   std::cout << "SAMPLE " << _sample_counter << " " << dot_samples << std::endl;
+      if ((_sample_counter%_sps) == 0) {
+        if (_symbol_counter == _symbols.size()) {
+          _symbol_counter = 0;
+          GILLock lock;
+          try {
+            boost::python::numpy::ndarray s = boost::python::numpy::from_data(&_descrambled_symbols.front(),
+                                                                              boost::python::numpy::dtype::get_builtin<gr_complex>(),
+                                                                              boost::python::make_tuple(_descrambled_symbols.size()),
+                                                                              boost::python::make_tuple(sizeof(gr_complex)),
+                                                                              boost::python::object());
+            update_doppler_information(_physicalLayer.attr("get_doppler")(s));
+            update_frame_information(_physicalLayer.attr("get_frame")());
+          } catch (boost::python::error_already_set const&) {
+            PyErr_Print();
+          }
+        }
+        gr_complex known_symbol = _symbols[_symbol_counter];
+        bool is_known = true;
+        for (int k=0; k<1; ++k) {
+          filter_output = 0;
+#if 1
+          volk_32fc_x2_dot_prod_32fc(&filter_output,
+                                     _hist_samples+_hist_sample_index,
+                                     _taps_samples,
+                                     _nB+_nF+1);
+#else
+          for (int l=0; l<_nB+_nF+1; ++l) {
+            assert(_hist_sample_index+l < 2*(_nB+_nF+1));
+            filter_output += _hist_samples[_hist_sample_index+l]*_taps_samples[l];
+          }
+#endif
+          gr_complex dot_symbols=0;
+          for (int l=0; l<_nW; ++l) {
+            assert(_hist_symbol_index+l < 2*_nW);
+            dot_symbols += _hist_symbols[_hist_symbol_index+l]*_taps_symbols[l];
+          }
+          filter_output += dot_symbols;
+          if (std::abs(known_symbol) < 1e-5) { // not known
+            is_known = false;
+            gr_complex descrambled_filter_output = std::conj(_scramble[_symbol_counter]) * filter_output;
+            gr::digital::constellation_sptr constell = _constellations[_constellation_index];
+            unsigned int jc = constell->decision_maker(&descrambled_filter_output);
+            constell->map_to_points(jc, &descrambled_filter_output);
+            known_symbol = _scramble[_symbol_counter] * descrambled_filter_output;
+          }
+          gr_complex err =  filter_output - known_symbol;
+          if (_symbol_counter >= 0) {
+            for (int j=0; j<_nB+_nF+1; ++j) {
+              _taps_samples[j] -= _mu*err*std::conj(_hist_samples[_hist_sample_index+j]);
+            }
+            for (int j=0; j<_nW; ++j) {
+              assert(_hist_symbol_index+j < 2*_nW);
+              _taps_symbols[j] -= _mu*err*std::conj(_hist_symbols[_hist_symbol_index+j]) + _alpha*_taps_symbols[j];
+            }
+          }
+          // std::cout << "filter: " << _symbol_counter << " " << _sample_counter << " " << filter_output << " " << known_symbol << " " << std::abs(err) << std::endl;
+        }
+        if (is_known) {
+          _taps_symbols[_hist_symbol_index] = _taps_symbols[_hist_symbol_index + _nW] = known_symbol;
+          if (++_hist_symbol_index == _nW)
+            _hist_symbol_index = 0;
+        }
+        _descrambled_symbols[_symbol_counter] = filter_output*std::conj(_scramble[_symbol_counter]);
+        out[nout++] = filter_output;
+        ++_symbol_counter;
+      }
+      _sample_counter += 1;
+    }
+  }
+
+  consume(0, i);
 
   // Tell runtime system how many output items we produced.
-  return noutput_items;
+  return nout;
 }
 
 bool adaptive_dfe_impl::start()
 {
+  // make sure python is ready for threading
+  if( Py_IsInitialized() ){
+    GILLock lock;
+    if(PyEval_ThreadsInitialized() != 1 ){
+      PyEval_InitThreads();
+    }
+    boost::python::numpy::initialize();
+  } else {
+    throw std::runtime_error("dont use es_pyhandler without python!");
+  }
+  _taps_samples = (gr_complex*)(volk_malloc(  (_nB+_nF+1)*sizeof(gr_complex), volk_get_alignment()));
+  _taps_symbols = (gr_complex*)(volk_malloc(          _nW*sizeof(gr_complex), volk_get_alignment()));
+  _hist_samples = (gr_complex*)(volk_malloc(2*(_nB+_nF+1)*sizeof(gr_complex), volk_get_alignment()));
+  _hist_symbols = (gr_complex*)(volk_malloc(        2*_nW*sizeof(gr_complex), volk_get_alignment()));
+  _taps_samples[_nB+1] = 1;
+  _taps_symbols[0]     = 1;
+
   std::cout << "adaptive_dfe_impl::start()" << std::endl;
   GILLock lock;
   try {
@@ -137,6 +301,10 @@ bool adaptive_dfe_impl::stop()
   std::cout << "adaptive_dfe_impl::stop()" << std::endl;
   GILLock lock;
   _physicalLayer = boost::python::object();
+  volk_free(_taps_samples);
+  volk_free(_taps_symbols);
+  volk_free(_hist_samples);
+  volk_free(_hist_symbols);
   return true;
 }
 
@@ -168,9 +336,11 @@ void adaptive_dfe_impl::update_frame_information(boost::python::object obj)
   int  const     m = array.shape(0);
   _symbols.resize(m);
   _scramble.resize(m);
+  _descrambled_symbols.resize(m);
   for (int i=0; i<m; ++i) {
     std::memcpy(&_symbols[i],  data+16*i,   sizeof(gr_complex));
     std::memcpy(&_scramble[i], data+16*i+8, sizeof(gr_complex));
+    // std::cout << "get_frame " << i << " " << _symbols[i] << " " << _scramble[i] << std::endl;
   }
   _constellation_index = boost::python::extract<int>(obj[1]);
 }
@@ -180,7 +350,17 @@ void adaptive_dfe_impl::update_doppler_information(boost::python::object obj)
   assert(n==2);
   double const do_continue = boost::python::extract<bool>(obj[0]);
   double const doppler = boost::python::extract<float>(obj[1]);
-  // TODO
+
+  float delta_f = doppler/_sps;
+  if (_df == 0) { // init
+    _ud = _df = -delta_f;
+  } else {
+    const float ud_old = _ud;
+    _ud  = -delta_f;
+    _df +=_b[0]*_ud + _b[1]*ud_old;
+  }
+  std::cout << "PLL: " << _df << " " << delta_f << std::endl;
+  _sum_phase_diff = 0;
 }
 
 } /* namespace digitalhf */

lib/adaptive_dfe_impl.h

@@ -34,19 +34,42 @@ private:
   int _sps;
   int _nB, _nF, _nW;
 
+  float _mu;
+  float _alpha;
+
   // module name w.r.t. digitalhf.physical_layer containing a PhysicalLayer class
   std::string           _py_module_name;
   boost::python::object _physicalLayer; // class instance of physical layer description
 
-  std::vector<gr_complex> _taps_samples;
+  gr_complex* _taps_samples;
-  std::vector<gr_complex> _taps_symbols;
+  gr_complex* _taps_symbols;
+
+  gr_complex* _hist_samples;
+  gr_complex* _hist_symbols;
+
+  int _hist_sample_index;
+  int _hist_symbol_index;
+
+  std::size_t _sample_counter;
 
   std::vector<gr::digital::constellation_sptr> _constellations;
   int _constellation_index;
   std::vector<gr_complex> _symbols;
   std::vector<gr_complex> _scramble;
+  std::vector<gr_complex> _descrambled_symbols;
+  int _symbol_counter;
 
-  int _sample_counter;
+  // PLL for doppler tracking
+  float _sum_phase_diff;
+  float _df;     // frequency offset in radians per sample
+  float _phase;  // accumulated phase for frequency correction
+  const float _b[2];
+  float _ud;
+
+  enum state {
+    WAIT_FOR_PREAMBLE,
+    DO_FILTER
+  } _state;
 
   void update_constellations(boost::python::object obj);
   void update_frame_information(boost::python::object obj);

python/physical_layer/STANAG_4285.py

@@ -14,8 +14,9 @@ class PhysicalLayer(object):
         self._preamble = [PhysicalLayer.get_preamble(), 0] ## BPSK
         self._data     = [PhysicalLayer.get_data(),  mode] ## according to the mode
         self._counter  = 0
+        self._preamble_phases = []
 
-    def set_mode(self):
+    def set_mode(self, mode):
         """For STANAG 4258 the mode has to be set manually: mode=0 -> BPSK, mode=1 -> QPSK, mode=2 -> 8PSK"""
         self._data[1] = mode
 
@@ -24,17 +25,32 @@ class PhysicalLayer(object):
 
     def get_frame(self):
         """returns the known+unknown symbols and scrambling"""
+        print('-------------------- get_frame --------------------',self._counter)
         if self._counter == 0:
-            return self._preamble
+            x= self._preamble
         else:
-            return self._data
+            x=self._data
+        print('get_frame end\n')
+        return x;
 
-    def get_doppler(self): ## symbols
+    def get_doppler(self, s):
         """used for doppler shift update, for determining which frame to provide next,
         and for stopping at end of data/when the signal quality is too low"""
+        print('-------------------- get_doppler --------------------',self._counter)
+        doppler = 0
+        if self._counter == 0: ## preamble
+            corr = s*np.conj(self._preamble[0]['symb'])
+            self._preamble_phases.extend([np.angle(np.sum(corr))])
+            if len(self._preamble_phases) == 2 and False:
+                doppler = 2*(self._preamble_phases[1] - self._preamble_phases[0])/256
+                print('preamble_phases', self._preamble_phases, 'doppler', doppler)
+                self._preamble_phases = self._preamble_phases[1:]
+            else:
+                phases = np.unwrap(np.angle(corr))
+                doppler = 2*(np.median(phases[-20:]) - np.median(phases[:20]))/80
+                print('doppler', doppler,self._preamble_phases)
+
         self._counter = (self._counter+1)&1
-        ## TODO: doppler calculations
-        doppler = 0.1234
         return [True, doppler]
 
     @staticmethod
@@ -67,9 +83,9 @@ class PhysicalLayer(object):
         ## PSK-8 modulation
         constellation = PhysicalLayer.make_psk(8,range(8))['points']
         a['scramble'] = constellation[p,]
-        a['symb'][ 32: 48] = 1 ## mini-probe 1
+        a['symb'][ 32: 48] = a['scramble'][ 32: 48] ## mini-probe 1
-        a['symb'][ 80: 96] = 1 ## mini-probe 2
+        a['symb'][ 80: 96] = a['scramble'][ 80: 96] ## mini-probe 2
-        a['symb'][128:144] = 1 ## mini-probe 3
+        a['symb'][128:144] = a['scramble'][128:144] ## mini-probe 3
         return a
 
     @staticmethod