clock_tracking_loop.cc

/* -*- c++ -*- */
/*
 * Copyright (C) 2011,2013,2016-2017 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 *
 */

#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,
                                         float ted_gain)
    : 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_ted_gain(ted_gain),
      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");

    if (d_ted_gain <= 0.0f)
        throw std::out_of_range(
            "clock_tracking_loop: expected ted gain 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, cosx_omega_d_T;
    float k0, k1, sinh_zeta_omega_n_T;
    float alpha, beta;

    omega_n_T = d_omega_n_norm;
    zeta_omega_n_T = d_zeta * omega_n_T;
    k0 = 2.0f / d_ted_gain;
    k1 = expf(-zeta_omega_n_T);
    sinh_zeta_omega_n_T = sinhf(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 = k0 * k1 * sinh_zeta_omega_n_T;
    beta = k0 * (1 - k1 * (sinh_zeta_omega_n_T + cosx_omega_d_T));

    set_alpha(alpha);
    set_beta(beta);
}

/*******************************************************************
 * 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_ted_gain(float ted_gain)
{
    if (ted_gain <= 0.0f)
        throw std::out_of_range("clock_tracking_loop: expected ted gain must be > 0.0");

    d_ted_gain = ted_gain;
    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 || 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_ted_gain() const { return d_ted_gain; }

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 */