Added a controlled fractional resampler

5 files changed, 330 insertions, 1 deletions

diff --git a/grc/CMakeLists.txt b/grc/CMakeLists.txt
index 47dfc89..1619f1a 100644
--- a/grc/CMakeLists.txt
+++ b/grc/CMakeLists.txt
@@ -25,5 +25,6 @@ add_subdirectory(flow_control)
 add_subdirectory(misc_utils)
 install(FILES
     gsm_block_tree.xml
-    grgsm_msg_to_tag.xml DESTINATION share/gnuradio/grc/blocks
+    grgsm_msg_to_tag.xml
+    grgsm_controlled_fractional_resampler_cc.xml DESTINATION share/gnuradio/grc/blocks
 )
diff --git a/grc/grgsm_controlled_fractional_resampler_cc.xml b/grc/grgsm_controlled_fractional_resampler_cc.xml
new file mode 100644
index 0000000..ca92572
--- /dev/null
+++ b/grc/grgsm_controlled_fractional_resampler_cc.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<block>
+  <name>controlled_fractional_resampler_cc</name>
+  <key>grgsm_controlled_fractional_resampler_cc</key>
+  <category>grgsm</category>
+  <import>import grgsm</import>
+  <make>grgsm.controlled_fractional_resampler_cc($phase_shift, $resamp_ratio)</make>
+	<callback>set_resamp_ratio($resamp_ratio)</callback>
+	<param>
+		<name>Phase Shift</name>
+		<key>phase_shift</key>
+		<type>real</type>
+	</param>
+	<param>
+		<name>Resampling Ratio</name>
+		<key>resamp_ratio</key>
+		<type>real</type>
+	</param>
+	<sink>
+		<name>in</name>
+		<type>complex</type>
+	</sink>
+	<source>
+		<name>out</name>
+		<type>complex</type>
+	</source>
+</block>
diff --git a/include/grgsm/controlled_fractional_resampler_cc.h b/include/grgsm/controlled_fractional_resampler_cc.h
new file mode 100644
index 0000000..9eeb393
--- /dev/null
+++ b/include/grgsm/controlled_fractional_resampler_cc.h
@@ -0,0 +1,64 @@
+/* -*- c++ -*- */
+/* @file
+ * @author Piotr Krysik <ptrkrysik@gmail.com>
+ * @section LICENSE
+ * 
+ * Gr-gsm is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ * 
+ * Gr-gsm is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with gr-gsm; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ * 
+ */
+
+
+#ifndef INCLUDED_GRGSM_CONTROLLED_FRACTIONAL_RESAMPLER_CC_H
+#define INCLUDED_GRGSM_CONTROLLED_FRACTIONAL_RESAMPLER_CC_H
+
+#include <grgsm/api.h>
+#include <gnuradio/block.h>
+//#include <gnuradio/filter/fractional_resampler_cc.h>
+
+namespace gr {
+  namespace gsm {
+
+    /*!
+     * \brief <+description of block+>
+     * \ingroup grgsm
+     *
+     */
+    class GRGSM_API controlled_fractional_resampler_cc :  virtual public block
+    {
+     public:
+      typedef boost::shared_ptr<controlled_fractional_resampler_cc> sptr;
+
+      /*!
+       * \brief Return a shared_ptr to a new instance of grgsm::controlled_fractional_resampler_cc.
+       *
+       * To avoid accidental use of raw pointers, grgsm::controlled_fractional_resampler_cc's
+       * constructor is in a private implementation
+       * class. grgsm::controlled_fractional_resampler_cc::make is the public interface for
+       * creating new instances.
+       */
+      static sptr make(float phase_shift, float resamp_ratio);
+      
+      virtual float mu() const = 0;
+      virtual float resamp_ratio() const = 0;
+      virtual void set_mu (float mu) = 0;
+      virtual void set_resamp_ratio(float resamp_ratio) = 0;
+    };
+
+  } // namespace grgsm
+} // namespace gr
+
+#endif /* INCLUDED_GRGSM_CONTROLLED_FRACTIONAL_RESAMPLER_CC_H */
+
diff --git a/lib/controlled_fractional_resampler_cc_impl.cc b/lib/controlled_fractional_resampler_cc_impl.cc
new file mode 100644
index 0000000..de35015
--- /dev/null
+++ b/lib/controlled_fractional_resampler_cc_impl.cc
@@ -0,0 +1,165 @@
+/* -*- c++ -*- */
+/* @file
+ * @author Piotr Krysik <ptrkrysik@gmail.com>
+ * @section LICENSE
+ * 
+ * Gr-gsm is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ * 
+ * Gr-gsm is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with gr-gsm; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ * 
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <gnuradio/io_signature.h>
+#include "controlled_fractional_resampler_cc_impl.h"
+#include <stdexcept>
+
+namespace gr {
+  namespace gsm {
+
+    controlled_fractional_resampler_cc::sptr
+    controlled_fractional_resampler_cc::make(float phase_shift, float resamp_ratio)
+    {
+      return gnuradio::get_initial_sptr
+        (new controlled_fractional_resampler_cc_impl(phase_shift, resamp_ratio));
+    }
+
+    controlled_fractional_resampler_cc_impl::controlled_fractional_resampler_cc_impl
+                                     (float phase_shift, float resamp_ratio)
+      : block("controlled_fractional_resampler_cc",
+              io_signature::make(1, 1, sizeof(gr_complex)),
+              io_signature::make(1, 1, sizeof(gr_complex))),
+        d_mu(phase_shift), d_mu_inc(resamp_ratio),
+        d_resamp(new mmse_fir_interpolator_cc())
+    {
+      this->set_tag_propagation_policy(TPP_DONT);
+      if(resamp_ratio <=  0)
+        throw std::out_of_range("resampling ratio must be > 0");
+      if(phase_shift <  0  || phase_shift > 1)
+        throw std::out_of_range("phase shift ratio must be > 0 and < 1");
+
+      set_relative_rate(1.0 / resamp_ratio);
+    }
+
+    controlled_fractional_resampler_cc_impl::~controlled_fractional_resampler_cc_impl()
+    {
+      delete d_resamp;
+    }
+
+    void
+    controlled_fractional_resampler_cc_impl::forecast(int noutput_items,
+                                           gr_vector_int &ninput_items_required)
+    {
+      unsigned ninputs = ninput_items_required.size();
+      for(unsigned i=0; i < ninputs; i++) {
+        ninput_items_required[i] =
+          (int)ceil((noutput_items * d_mu_inc) + d_resamp->ntaps());
+      }
+    }
+
+    int
+    controlled_fractional_resampler_cc_impl::general_work(int noutput_items,
+                                               gr_vector_int &ninput_items,
+                                               gr_vector_const_void_star &input_items,
+                                               gr_vector_void_star &output_items)
+    {
+      const gr_complex *in = (const gr_complex*)input_items[0];
+      gr_complex *out = (gr_complex*)output_items[0];
+      
+      uint64_t processed_in = 0; //input samples processed in the last call to resample function
+      uint64_t processed_in_sum = 0; //input samples processed during a whole call to general_work function
+      uint64_t produced_out_sum = 0; //output samples produced during a whole call to general_work function
+
+      std::vector<tag_t> set_resamp_ratio_tags;
+
+      pmt::pmt_t key = pmt::string_to_symbol("set_resamp_ratio");
+      get_tags_in_window(set_resamp_ratio_tags, 0, 0, noutput_items, key);
+
+      std::cout << "-----------------------------" << std::endl;        
+      
+      for(std::vector<tag_t>::iterator i_tag = set_resamp_ratio_tags.begin(); i_tag < set_resamp_ratio_tags.end(); i_tag++)
+      {
+        uint64_t tag_offset_rel = i_tag->offset-nitems_read(0);
+        uint64_t samples_to_produce = (tag_offset_rel-processed_in_sum)/d_mu_inc; //tu może być problem
+        std::cout << "samples_to_produce = (tag_offset_rel-processed_in_sum)/d_mu_inc: " << samples_to_produce << " = " << tag_offset_rel << " - " << processed_in_sum << std::endl;
+        processed_in = resample(in, processed_in_sum, out, produced_out_sum, samples_to_produce);
+        processed_in_sum = processed_in_sum + processed_in;
+        produced_out_sum = produced_out_sum + samples_to_produce;
+        std::cout << "processed_in: " << processed_in <<   " tag_offset_rel: " << tag_offset_rel << " produced_out_sum: " << produced_out_sum << std::endl;        
+        set_resamp_ratio(pmt::to_double(i_tag->value));
+//        std::cout << "Setting resamp ratio: " << d_mu_inc << std::endl;
+      }
+
+      std::cout << "noutput_items: " << noutput_items << " produced_out_sum: " << produced_out_sum << std::endl;
+      std::cout << "last_resample_outputs: " << (noutput_items-produced_out_sum) << std::endl;
+//      processed_in = resample(in, processed_in_sum, out, produced_out_sum, samples_to_produce);
+//      processed_in_sum = processed_in_sum + processed_in;
+//      produced_out_sum = produced_out_sum + samples_to_produce;
+
+      processed_in = resample(in, processed_in_sum, out, produced_out_sum, (noutput_items-produced_out_sum));
+//      processed_in = resample(in, 0, out, 0, noutput_items);
+      processed_in_sum = processed_in_sum + processed_in;
+      consume_each(processed_in_sum);
+      return noutput_items;
+    }
+    
+    inline uint64_t 
+    controlled_fractional_resampler_cc_impl::resample(const gr_complex *in, uint64_t first_in_sample, gr_complex *out, uint64_t first_out_sample, uint64_t samples_to_produce)
+    {
+      int ii = first_in_sample;
+      int oo = first_out_sample;
+      while(oo < (first_out_sample+samples_to_produce)) //produce samples_to_produce number of samples
+      {
+        out[oo++] = d_resamp->interpolate(&in[ii], d_mu);
+      
+        double s = d_mu + d_mu_inc;
+        double f = floor(s);
+        int incr = (int)f;
+        d_mu = s - f;
+        ii += incr;
+      }
+      return ii-first_in_sample; //number of input samples processed
+    }
+
+    float
+    controlled_fractional_resampler_cc_impl::mu() const
+    {
+      return d_mu;
+    }
+
+    float
+    controlled_fractional_resampler_cc_impl::resamp_ratio() const
+    {
+      return d_mu_inc;
+    }
+
+    void
+    controlled_fractional_resampler_cc_impl::set_mu(float mu)
+    {
+      d_mu = mu;
+    }
+
+    void
+    controlled_fractional_resampler_cc_impl::set_resamp_ratio(float resamp_ratio)
+    {
+      d_mu_inc = resamp_ratio;
+      set_relative_rate(1.0 / resamp_ratio);
+    }
+
+  } /* namespace grgsm */
+} /* namespace gr */
+
diff --git a/lib/controlled_fractional_resampler_cc_impl.h b/lib/controlled_fractional_resampler_cc_impl.h
new file mode 100644
index 0000000..73083ea
--- /dev/null
+++ b/lib/controlled_fractional_resampler_cc_impl.h
@@ -0,0 +1,72 @@
+/* -*- c++ -*- */
+/* @file
+ * @author Piotr Krysik <ptrkrysik@gmail.com>
+ * @section LICENSE
+ * 
+ * Gr-gsm is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ * 
+ * Gr-gsm is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with gr-gsm; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ * 
+ */
+
+#ifndef INCLUDED_GRGSM_CONTROLLED_FRACTIONAL_RESAMPLER_CC_IMPL_H
+#define INCLUDED_GRGSM_CONTROLLED_FRACTIONAL_RESAMPLER_CC_IMPL_H
+
+#include <grgsm/controlled_fractional_resampler_cc.h>
+//#include <gnuradio/filter/fractional_resampler_cc.h>
+#include <gnuradio/filter/mmse_fir_interpolator_cc.h>
+
+using namespace gr::filter;
+
+namespace gr {
+  namespace gsm {
+
+    class controlled_fractional_resampler_cc_impl : public controlled_fractional_resampler_cc
+    {
+    private:
+      float d_mu;
+      float d_mu_inc;
+      mmse_fir_interpolator_cc *d_resamp;
+
+      inline uint64_t resample(const gr_complex *in, 
+           uint64_t first_in_sample, 
+           gr_complex *out, 
+           uint64_t first_out_sample, 
+           uint64_t samples_to_produce);
+                                               
+    public:
+      controlled_fractional_resampler_cc_impl(float phase_shift,
+                                   float resamp_ratio);
+      ~controlled_fractional_resampler_cc_impl();
+
+      void forecast(int noutput_items,
+		       gr_vector_int &ninput_items_required);
+
+      int general_work(int noutput_items,
+		       gr_vector_int &ninput_items,
+		       gr_vector_const_void_star &input_items,
+		       gr_vector_void_star &output_items);
+
+
+      float mu() const;
+      float resamp_ratio() const;
+      void set_mu(float mu);
+      void set_resamp_ratio(float resamp_ratio);
+    };
+
+  } // namespace grgsm
+} // namespace gr
+
+#endif /* INCLUDED_GRGSM_CONTROLLED_FRACTIONAL_RESAMPLER_CC_IMPL_H */
+