1 files changed, 108 insertions, 0 deletions
diff --git a/gr-channels/lib/cfo_model_impl.cc b/gr-channels/lib/cfo_model_impl.cc
new file mode 100644
index 0000000000..dc4f829dc0
--- /dev/null
+++ b/gr-channels/lib/cfo_model_impl.cc
@@ -0,0 +1,108 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2009,2012 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * GNU Radio 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.
+ *
+ * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#include "cfo_model_impl.h"
+#include <gnuradio/io_signature.h>
+#include <iostream>
+
+namespace gr {
+  namespace channels {
+    
+    cfo_model::sptr
+    cfo_model::make(
+                double sample_rate_hz,
+                double std_dev_hz,
+                double max_dev_hz,
+                double noise_seed )
+    {
+      return gnuradio::get_initial_sptr
+	(new cfo_model_impl(sample_rate_hz,
+				std_dev_hz,
+                max_dev_hz,
+				noise_seed));
+    }
+
+    cfo_model_impl::cfo_model_impl(
+                       double sample_rate_hz,
+                       double std_dev_hz,
+                       double max_dev_hz,
+					   double noise_seed)
+      : sync_block("cfo_model",
+		       io_signature::make(1, 1, sizeof(gr_complex)),
+		       io_signature::make(1, 1, sizeof(gr_complex))),
+        d_samp_rate(sample_rate_hz),
+        d_max_dev_hz(max_dev_hz),
+        d_table(8*1024),
+        d_noise(gr::analog::fastnoise_source_f::make(analog::GR_GAUSSIAN, std_dev_hz, noise_seed)),
+        d_cfo(0),
+        d_angle(0)
+    {
+    }
+
+    cfo_model_impl::~cfo_model_impl()
+    {
+    }
+
+    int cfo_model_impl::work (int noutput_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];
+        for(size_t i=0; i<noutput_items; i++){
+            // update and bound cfo
+            // we multiply by a random {1,-1} to remove any sign
+            // bias that may exist in our random sample pool
+            d_cfo += d_noise->sample_unbiased();
+            d_cfo = std::min( d_cfo, d_max_dev_hz );
+            d_cfo = std::max( d_cfo, -d_max_dev_hz );
+            // update and wrap angle
+            d_angle += 2*M_PI*d_cfo/d_samp_rate;
+            d_angle = d_angle > 2*M_PI ? d_angle - 2*M_PI : d_angle;
+            d_angle = d_angle < -2*M_PI ? d_angle + 2*M_PI : d_angle;
+            out[i] = in[i] * gr_complex(d_table.cos(d_angle), d_table.sin(d_angle));
+        }
+        return noutput_items;
+       
+    }
+
+    void
+    cfo_model_impl::setup_rpc()
+    {
+#ifdef GR_CTRLPORT
+      add_rpc_variable(
+        rpcbasic_sptr(new rpcbasic_register_variable<double>(
+	  alias(), "cfo", &d_cfo,
+	  pmt::mp(-10.0f), pmt::mp(10.0f), pmt::mp(0.0f),
+	  "", "Current CFO in Hz", RPC_PRIVLVL_MIN,
+          DISPTIME | DISPOPTSTRIP))); 
+      add_rpc_variable(
+        rpcbasic_sptr(new rpcbasic_register_variable<double>(
+	  alias(), "cfo_max", &d_max_dev_hz,
+	  pmt::mp(-10.0f), pmt::mp(10.0f), pmt::mp(0.0f),
+	  "", "Max CFO in Hz", RPC_PRIVLVL_MIN,
+          DISPTIME | DISPOPTSTRIP))); 
+#endif /* GR_CTRLPORT */
+    }
+
+  } /* namespace channels */
+} /* namespace gr */