Adding a control_loop class that takes care of setting the loop gains and the standard loop advance function. This should be used as a parent class of any block that uses a 2nd order control loop (like the PLL, clock recovery, etc blocks).

5 files changed, 446 insertions, 0 deletions

diff --git a/gnuradio-core/src/lib/general/Makefile.am b/gnuradio-core/src/lib/general/Makefile.am
index 29f9d8d44..f210cee39 100644
--- a/gnuradio-core/src/lib/general/Makefile.am
+++ b/gnuradio-core/src/lib/general/Makefile.am
@@ -153,6 +153,7 @@ libgeneral_la_SOURCES = 		\
 	gr_wvps_ff.cc			\
 	gri_add_const_ss_generic.cc	\
 	gri_char_to_float.cc		\
+	gri_control_loop.cc		\
 	gri_debugger_hook.cc		\
 	gri_fft.cc			\
 	gri_float_to_char.cc		\
@@ -312,6 +313,7 @@ grinclude_HEADERS = 			\
 	gri_agc2_cc.h			\
 	gri_agc2_ff.h			\
 	gri_char_to_float.h		\
+	gri_control_loop.h		\
 	gri_debugger_hook.h		\
 	gri_fft.h			\
 	gri_float_to_char.h		\
@@ -460,6 +462,7 @@ swiginclude_HEADERS =			\
 	gri_agc_ff.i			\
 	gri_agc2_cc.i			\
 	gri_agc2_ff.i			\
+	gri_control_loop.i		\
 	gr_descrambler_bb.i		\
 	gr_scrambler_bb.i		\
 	gr_probe_mpsk_snr_c.i		\
diff --git a/gnuradio-core/src/lib/general/general.i b/gnuradio-core/src/lib/general/general.i
index 08b5ad98c..f9adff3fa 100644
--- a/gnuradio-core/src/lib/general/general.i
+++ b/gnuradio-core/src/lib/general/general.i
@@ -22,6 +22,7 @@
 
 %{
 
+#include <gri_control_loop.h>
 #include <gr_nop.h>
 #include <gr_null_sink.h>
 #include <gr_null_source.h>
@@ -138,6 +139,7 @@
 #include <gr_burst_tagger.h>
 %}
 
+%include "gri_control_loop.i"
 %include "gr_nop.i"
 %include "gr_null_sink.i"
 %include "gr_null_source.i"
diff --git a/gnuradio-core/src/lib/general/gri_control_loop.cc b/gnuradio-core/src/lib/general/gri_control_loop.cc
new file mode 100644
index 000000000..d84b47493
--- /dev/null
+++ b/gnuradio-core/src/lib/general/gri_control_loop.cc
@@ -0,0 +1,183 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2011 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 <gri_control_loop.h>
+#include <gr_math.h>
+#include <stdexcept>
+
+#define M_TWOPI (2.0f*M_PI)
+
+gri_control_loop::gri_control_loop(float loop_bw,
+				   float max_freq, float min_freq)
+  : d_phase(0), d_freq(0), d_max_freq(max_freq), d_min_freq(min_freq)
+{
+  // Set the damping factor for a critically damped system
+  d_damping = sqrtf(2.0f)/2.0f;
+
+  // Set the bandwidth, which will then call update_gains()
+  set_loop_bandwidth(loop_bw);
+}
+
+gri_control_loop::~gri_control_loop()
+{
+}
+
+void
+gri_control_loop::update_gains()
+{
+  float denom = (1.0 + 2.0*d_damping*d_loop_bw + d_loop_bw*d_loop_bw);
+  d_alpha = (4*d_damping*d_loop_bw) / denom;
+  d_beta = (4*d_loop_bw*d_loop_bw) / denom;
+}
+
+void
+gri_control_loop::advance_loop(float error)
+{
+  d_freq = d_freq + d_beta * error;
+  d_phase = d_phase + d_freq + d_alpha * error;
+}
+
+
+void
+gri_control_loop::phase_wrap()
+{
+  while(d_phase>M_TWOPI)
+    d_phase -= M_TWOPI;
+  while(d_phase<-M_TWOPI)
+    d_phase += M_TWOPI;
+}
+
+void
+gri_control_loop::frequency_limit()
+{  
+  if (d_freq > d_max_freq)
+    d_freq = d_min_freq;
+  else if (d_freq < d_min_freq)
+    d_freq = d_max_freq;  
+}
+
+/*******************************************************************
+    SET FUNCTIONS
+*******************************************************************/
+
+void
+gri_control_loop::set_loop_bandwidth(float bw)
+{
+  if(bw < 0) {
+    throw std::out_of_range ("gri_control_loop: invalid bandwidth. Must be >= 0.");
+  }
+  
+  d_loop_bw = bw;
+  update_gains();
+}
+
+void
+gri_control_loop::set_damping_factor(float df) 
+{
+  if(df < 0 || df > 1.0) {
+    throw std::out_of_range ("gri_control_loop: invalid damping factor. Must be in [0,1].");
+  }
+  
+  d_damping = df;
+  update_gains();
+}
+
+void
+gri_control_loop::set_alpha(float alpha)
+{
+  if(alpha < 0 || alpha > 1.0) {
+    throw std::out_of_range ("gri_control_loop: invalid alpha. Must be in [0,1].");
+  }
+  d_alpha = alpha;
+}
+
+void
+gri_control_loop::set_beta(float beta)
+{
+  if(beta < 0 || beta > 1.0) {
+    throw std::out_of_range ("gri_control_loop: invalid beta. Must be in [0,1].");
+  }
+  d_beta = beta;
+}
+
+void
+gri_control_loop::set_frequency(float freq)
+{
+  if(freq > d_max_freq)
+    d_freq = d_min_freq;
+  else if(freq < d_min_freq)
+    d_freq = d_max_freq;
+  else
+    d_freq = freq;
+}
+
+void
+gri_control_loop::set_phase(float phase)
+{
+  d_phase = phase;
+  while(d_phase>M_TWOPI)
+    d_phase -= M_TWOPI;
+  while(d_phase<-M_TWOPI)
+    d_phase += M_TWOPI;
+}
+
+   
+/*******************************************************************
+    GET FUNCTIONS
+*******************************************************************/
+
+
+float
+gri_control_loop::get_loop_bandwidth() const
+{
+  return d_loop_bw;
+}
+
+float
+gri_control_loop::get_damping_factor() const
+{
+  return d_damping;
+}
+
+float
+gri_control_loop::get_alpha() const
+{
+  return d_alpha;
+}
+
+float
+gri_control_loop::get_beta() const
+{
+  return d_beta;
+}
+
+float
+gri_control_loop::get_frequency() const
+{
+  return d_freq;
+}
+
+float
+gri_control_loop::get_phase() const
+{
+  return d_phase;
+}
diff --git a/gnuradio-core/src/lib/general/gri_control_loop.h b/gnuradio-core/src/lib/general/gri_control_loop.h
new file mode 100644
index 000000000..a85625bbd
--- /dev/null
+++ b/gnuradio-core/src/lib/general/gri_control_loop.h
@@ -0,0 +1,201 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2011 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.
+ */
+
+#ifndef GRI_CONTROL_LOOP
+#define GRI_CONTROL_LOOP
+
+class gri_control_loop
+{
+ protected:
+  float d_phase, d_freq;
+  float d_max_freq, d_min_freq;
+  float d_damping, d_loop_bw;
+  float d_alpha, d_beta;
+
+ public:
+  gri_control_loop(float loop_bw, float max_freq, float min_freq);
+  virtual ~gri_control_loop();
+
+  /*! \brief update the system gains from the loop bandwidth and damping factor
+   *
+   *  This function updates the system gains based on the loop
+   *  bandwidth and damping factor of the system.
+   *  These two factors can be set separately through their own
+   *  set functions.
+   */
+  void update_gains();
+
+  /*! \brief update the system gains from the loop bandwidth and damping factor
+   *
+   *  This function updates the system gains based on the loop
+   *  bandwidth and damping factor of the system.
+   *  These two factors can be set separately through their own
+   *  set functions.
+   */
+  void advance_loop(float error);
+
+  /*! \brief Keep the phase between -2pi and 2pi
+   *
+   * This function keeps the phase between -2pi and 2pi. If the phase
+   * is greater than 2pi by d, it wraps around to be -2pi+d; similarly if
+   * it is less than -2pi by d, it wraps around to 2pi-d.
+   *
+   * This function should be called after advance_loop to keep the phase
+   * in a good operating region. It is set as a separate method in case
+   * another way is desired as this is fairly heavy-handed.
+   */
+  void phase_wrap();
+
+  /*! \brief Keep the frequency between d_min_freq and d_max_freq
+   *
+   * This function keeps the frequency between d_min_freq and d_max_freq.
+   * If the frequency is greater than d_max_freq, it is set to d_max_freq.
+   * If the frequency is less than d_min_freq, it is set to d_min_freq.
+   *
+   * This function should be called after advance_loop to keep the frequency
+   * in the specified region. It is set as a separate method in case
+   * another way is desired as this is fairly heavy-handed.
+   */
+  void frequency_limit();
+
+  /*******************************************************************
+    SET FUNCTIONS
+  *******************************************************************/
+
+  /*!
+   * \brief Set the loop bandwidth
+   *
+   * Set the loop filter's bandwidth to \p bw. This should be between 
+   * 2*pi/200 and 2*pi/100  (in rads/samp). It must also be a positive
+   * number.
+   *
+   * When a new damping factor is set, the gains, alpha and beta, of the loop
+   * are recalculated by a call to update_gains().
+   *
+   * \param bw    (float) new bandwidth
+   *
+   */
+  void set_loop_bandwidth(float bw);
+
+  /*!
+   * \brief Set the loop damping factor
+   *
+   * Set the loop filter's damping factor to \p df. The damping factor
+   * should be sqrt(2)/2.0 for critically damped systems.
+   * Set it to anything else only if you know what you are doing. It must
+   * be a number between 0 and 1.
+   *
+   * When a new damping factor is set, the gains, alpha and beta, of the loop
+   * are recalculated by a call to update_gains().
+   *
+   * \param df    (float) new damping factor
+   *
+   */
+  void set_damping_factor(float df);
+
+  /*!
+   * \brief Set the loop gain alpha
+   *
+   * Set's the loop filter's alpha gain parameter.
+   *
+   * This value should really only be set by adjusting the loop bandwidth
+   * and damping factor.
+   *
+   * \param alpha    (float) new alpha gain
+   *
+   */
+  void set_alpha(float alpha);
+
+  /*!
+   * \brief Set the loop gain beta
+   *
+   * Set's the loop filter's beta gain parameter.
+   *
+   * This value should really only be set by adjusting the loop bandwidth
+   * and damping factor.
+   *
+   * \param beta    (float) new beta gain
+   *
+   */
+  void set_beta(float beta);
+
+  /*!
+   * \brief Set the Costas loop's frequency.
+   *
+   * Set's the Costas Loop's frequency. While this is normally updated by the
+   * inner loop of the algorithm, it could be useful to manually initialize,
+   * set, or reset this under certain circumstances.
+   *
+   * \param freq    (float) new frequency
+   *
+   */
+  void set_frequency(float freq);
+
+  /*!
+   * \brief Set the Costas loop's phase.
+   *
+   * Set's the Costas Loop's phase. While this is normally updated by the
+   * inner loop of the algorithm, it could be useful to manually initialize,
+   * set, or reset this under certain circumstances.
+   *
+   * \param phase    (float) new phase
+   *
+   */
+  void set_phase(float phase);
+
+   
+  /*******************************************************************
+    GET FUNCTIONS
+  *******************************************************************/
+
+  /*!
+   * \brief Returns the loop bandwidth
+   */
+  float get_loop_bandwidth() const;
+
+  /*!
+   * \brief Returns the loop damping factor
+   */
+  float get_damping_factor() const;
+
+  /*!
+   * \brief Returns the loop gain alpha
+   */
+  float get_alpha() const;
+
+  /*!
+   * \brief Returns the loop gain beta
+   */
+  float get_beta() const;
+
+  /*!
+   * \brief Get the Costas loop's frequency estimate
+   */
+  float get_frequency() const;
+
+  /*!
+   * \brief Get the Costas loop's phase estimate
+   */
+  float get_phase() const;
+};
+
+#endif /* GRI_CONTROL_LOOP */
diff --git a/gnuradio-core/src/lib/general/gri_control_loop.i b/gnuradio-core/src/lib/general/gri_control_loop.i
new file mode 100644
index 000000000..67f8838cb
--- /dev/null
+++ b/gnuradio-core/src/lib/general/gri_control_loop.i
@@ -0,0 +1,57 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2011 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.
+ */
+
+
+class gri_control_loop
+{
+ public:
+  gri_control_loop(float loop_bw, float max_freq, float min_freq);
+  virtual ~gri_control_loop();
+
+  void update_gains();
+  void advance_loop(float error);
+  void phase_wrap();
+  void frequency_limit();
+
+  /*******************************************************************
+    SET FUNCTIONS
+  *******************************************************************/
+
+  void set_loop_bandwidth(float bw);
+  void set_damping_factor(float df);
+  void set_alpha(float alpha);
+  void set_beta(float beta);
+  void set_frequency(float freq);
+  void set_phase(float phase);
+
+   
+  /*******************************************************************
+    GET FUNCTIONS
+  *******************************************************************/
+
+  float get_loop_bandwidth() const;
+  float get_damping_factor() const;
+  float get_alpha() const;
+  float get_beta() const;
+  float get_frequency() const;
+  float get_phase() const;
+};