diff options
Diffstat (limited to 'gr-digital/lib/clock_tracking_loop.cc')
| -rw-r--r-- | gr-digital/lib/clock_tracking_loop.cc | 307 |
1 files changed, 307 insertions, 0 deletions
diff --git a/gr-digital/lib/clock_tracking_loop.cc b/gr-digital/lib/clock_tracking_loop.cc new file mode 100644 index 0000000000..d74e4b2120 --- /dev/null +++ b/gr-digital/lib/clock_tracking_loop.cc @@ -0,0 +1,307 @@ +/* -*- c++ -*- */ +/* + * Copyright (C) 2011,2013,2016-2017 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. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include "clock_tracking_loop.h" +#include <gnuradio/math.h> +#include <stdexcept> + +namespace gr { + namespace digital { + + clock_tracking_loop::clock_tracking_loop(float loop_bw, + float max_period, float min_period, + float nominal_period, + float damping) + : d_avg_period(nominal_period), + d_max_avg_period(max_period), + d_min_avg_period(min_period), + d_nom_avg_period(nominal_period), + d_inst_period(nominal_period), + d_phase(0.0f), + d_zeta(damping), + d_omega_n_norm(loop_bw), + d_alpha(0.0f), + d_beta(0.0f), + d_prev_avg_period(nominal_period), + d_prev_inst_period(nominal_period), + d_prev_phase(0.0f) + { + set_max_avg_period(max_period); + set_min_avg_period(min_period); + set_nom_avg_period(nominal_period); + + set_avg_period(d_nom_avg_period); + set_inst_period(d_nom_avg_period); + + if (d_zeta < 0.0f) + throw std::out_of_range ( + "clock_tracking_loop: damping factor must be > 0.0"); + + if (d_omega_n_norm < 0.0f) + throw std::out_of_range ( + "clock_tracking_loop: loop bandwidth must be greater than 0.0"); + + update_gains(); + } + + clock_tracking_loop::~clock_tracking_loop() + { + } + + void + clock_tracking_loop::update_gains() + { + float omega_n_T, omega_d_T, zeta_omega_n_T, k1, cosx_omega_d_T; + float alpha, beta; + + omega_n_T = d_omega_n_norm; + zeta_omega_n_T = d_zeta * omega_n_T; + k1 = 2.0f * expf(-zeta_omega_n_T); + + if (d_zeta > 1.0f) { // Over-damped (or critically-damped too) + + omega_d_T = omega_n_T * sqrtf(d_zeta * d_zeta - 1.0f); + cosx_omega_d_T = coshf(omega_d_T); + // cosh ---------^^^^ + + } else if (d_zeta == 1.0f) { // Critically-damped + + omega_d_T = 0.0f; + cosx_omega_d_T = 1.0f; + // cosh(omega_d_T) & cos(omega_d_T) are both 1 for omega_d_T == 0 + + } else { // Under-damped (or critically-damped too) + + omega_d_T = omega_n_T * sqrtf(1.0 - d_zeta * d_zeta); + cosx_omega_d_T = cosf(omega_d_T); + // cos ----------^^^ + } + + alpha = k1 * sinhf(zeta_omega_n_T); + beta = 2.0f - (alpha + k1 * cosx_omega_d_T); + + set_alpha(alpha); + set_beta(beta); + } + + void + clock_tracking_loop::advance_loop(float error) + { + // So the loop can be reverted one step, if needed. + d_prev_avg_period = d_avg_period; + d_prev_inst_period = d_inst_period; + d_prev_phase = d_phase; + + // Integral arm of PI filter + d_avg_period = d_avg_period + d_beta * error; + // Proportional arm of PI filter and final sum of PI filter arms + d_inst_period = d_avg_period + d_alpha * error; + // Compute the new, unwrapped clock phase + d_phase = d_phase + d_inst_period; + } + + void + clock_tracking_loop::revert_loop() + { + d_avg_period = d_prev_avg_period; + d_inst_period = d_prev_inst_period; + d_phase = d_prev_phase; + } + + void + clock_tracking_loop::phase_wrap() + { + float period = d_avg_period; // One could argue d_inst_period instead + float limit = period/2.0f; + + while (d_phase > limit) + d_phase -= period; + + while (d_phase <= -limit) + d_phase += period; + } + + void + clock_tracking_loop::period_limit() + { + if (d_avg_period > d_max_avg_period) + d_avg_period = d_max_avg_period; + else if (d_avg_period < d_min_avg_period) + d_avg_period = d_min_avg_period; + } + + /******************************************************************* + * SET FUNCTIONS + *******************************************************************/ + + void + clock_tracking_loop::set_loop_bandwidth(float bw) + { + if (bw < 0.0f) + throw std::out_of_range ( + "clock_tracking_loop: loop bandwidth must be greater than 0.0"); + + d_omega_n_norm = bw; + update_gains(); + } + + void + clock_tracking_loop::set_damping_factor(float df) + { + if (df < 0.0f) + throw std::out_of_range ( + "clock_tracking_loop: damping factor must be > 0.0"); + + d_zeta = df; + update_gains(); + } + + void + clock_tracking_loop::set_alpha(float alpha) + { + d_alpha = alpha; + } + + void + clock_tracking_loop::set_beta(float beta) + { + d_beta = beta; + } + + void + clock_tracking_loop::set_avg_period(float period) + { + d_avg_period = period; + d_prev_avg_period = period; + } + + void + clock_tracking_loop::set_inst_period(float period) + { + d_inst_period = period; + d_prev_inst_period = period; + } + + void + clock_tracking_loop::set_phase(float phase) + { + // This previous phase is likely inconsistent with the tracking, + // but if the caller is setting the phase, the odds of + // revert_loop() being called are slim. + d_prev_phase = phase; + + d_phase = phase; + } + + void + clock_tracking_loop::set_max_avg_period(float period) + { + d_max_avg_period = period; + } + + void + clock_tracking_loop::set_min_avg_period(float period) + { + d_min_avg_period = period; + } + + void + clock_tracking_loop::set_nom_avg_period(float period) + { + if (period < d_min_avg_period or + period > d_max_avg_period ) { + d_nom_avg_period = (d_max_avg_period + d_min_avg_period)/2.0f; + } else { + d_nom_avg_period = period; + } + } + + /******************************************************************* + * GET FUNCTIONS + *******************************************************************/ + + float + clock_tracking_loop::get_loop_bandwidth() const + { + return d_omega_n_norm; + } + + float + clock_tracking_loop::get_damping_factor() const + { + return d_zeta; + } + + float + clock_tracking_loop::get_alpha() const + { + return d_alpha; + } + + float + clock_tracking_loop::get_beta() const + { + return d_beta; + } + + float + clock_tracking_loop::get_avg_period() const + { + return d_avg_period; + } + + float + clock_tracking_loop::get_inst_period() const + { + return d_inst_period; + } + + float + clock_tracking_loop::get_phase() const + { + return d_phase; + } + + float + clock_tracking_loop::get_max_avg_period() const + { + return d_max_avg_period; + } + + float + clock_tracking_loop::get_min_avg_period() const + { + return d_min_avg_period; + } + + float + clock_tracking_loop::get_nom_avg_period() const + { + return d_nom_avg_period; + } + + } /* namespace digital */ +} /* namespace gr */ |