mirror of https://github.com/hb9fxq/gr-digitalhf
132 lines
3.7 KiB
C++
132 lines
3.7 KiB
C++
/* -*- c++ -*- */
|
|
/*
|
|
* Copyright 2018 hcab14@gmail.com.
|
|
*
|
|
* This 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.
|
|
*
|
|
* This software 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 this software; 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 <cmath>
|
|
|
|
#include <gnuradio/io_signature.h>
|
|
#include <gnuradio/expj.h>
|
|
#include <gnuradio/logger.h>
|
|
|
|
#include "vector_pll_cc_impl.h"
|
|
|
|
namespace gr {
|
|
namespace digitalhf {
|
|
|
|
vector_pll_cc::sptr
|
|
vector_pll_cc::make(float samp_rate,
|
|
size_t order,
|
|
std::vector<int> freq_multipliers)
|
|
{
|
|
return gnuradio::get_initial_sptr
|
|
(new vector_pll_cc_impl(samp_rate,
|
|
order,
|
|
freq_multipliers));
|
|
}
|
|
|
|
/*
|
|
* The private constructor
|
|
*/
|
|
vector_pll_cc_impl::vector_pll_cc_impl(float samp_rate,
|
|
size_t order,
|
|
std::vector<int> freq_multipliers)
|
|
: gr::sync_block("vector_pll_cc",
|
|
gr::io_signature::make(1, 1, freq_multipliers.size() * sizeof(gr_complex)),
|
|
gr::io_signature::make(1, 1, freq_multipliers.size() * sizeof(gr_complex)))
|
|
, _ts(1.0/samp_rate)
|
|
, _order(order)
|
|
, _freq_multipliers(freq_multipliers)
|
|
, _control_loop(6.28/600.0, -6.28/600.0*10, 6.28/600.0*10)
|
|
, _theta (freq_multipliers.size(), 0.0f)
|
|
, _dtheta(freq_multipliers.size(), 0.0f)
|
|
, _absz(freq_multipliers.size(), 1.0f)
|
|
, _b({+0.6284830097698858,
|
|
-0.6281540229565162})
|
|
, _uf(0)
|
|
, _ud(0)
|
|
{
|
|
GR_LOG_DECLARE_LOGPTR(d_logger);
|
|
GR_LOG_ASSIGN_LOGPTR(d_logger, "vector_pll_cc");
|
|
for (int i=0; i<_freq_multipliers.size(); ++i) {
|
|
std::cout << "FM: " << i <<" " << _freq_multipliers[i] << std::endl;
|
|
}
|
|
}
|
|
|
|
vector_pll_cc_impl::~vector_pll_cc_impl()
|
|
{
|
|
}
|
|
|
|
int
|
|
vector_pll_cc_impl::work(int noutput_items,
|
|
gr_vector_const_void_star &input_items,
|
|
gr_vector_void_star &output_items)
|
|
{
|
|
gr::thread::scoped_lock lock(d_setlock);
|
|
|
|
size_t const vlen = _freq_multipliers.size();
|
|
const gr_complex *in = (const gr_complex*)input_items[0];
|
|
gr_complex *out = (gr_complex*)output_items[0];
|
|
_control_loop.frequency_limit();
|
|
|
|
std::vector<float> error(vlen);
|
|
|
|
float const mu = 5e-2;
|
|
|
|
for (int i=0; i<noutput_items; ++i) {
|
|
for (int j=0; j<vlen; ++j) {
|
|
_theta[j] += _uf * _freq_multipliers[j] * _ts + _dtheta[j];
|
|
_theta[j] = std::fmod(_theta[j], 2*M_PI*_order);
|
|
out[j] = gr_expj(-_theta[j]/_order) * in[j];
|
|
}
|
|
|
|
float weight=0, sum_weights=0;
|
|
for (int j=0; j<vlen; ++j) {
|
|
gr_complex z = in[j];
|
|
for (size_t k=1; k<_order; k*=2)
|
|
z *= z;
|
|
_absz[j] = weight = 0.99*_absz[j] + 0.01*std::abs(z);
|
|
sum_weights += weight;
|
|
gr_complex const x = gr_expj(-_theta[j]) * z;
|
|
error[j] = std::imag(x)/std::abs(x) * weight;
|
|
}
|
|
|
|
float sum_error = 0;
|
|
for (int j=0; j<vlen; ++j) {
|
|
error[j] *= vlen / sum_weights;
|
|
sum_error += error[j];
|
|
_dtheta[j] = mu * error[j];
|
|
}
|
|
|
|
float const ud = sum_error / vlen;
|
|
_uf += _b[0]*ud + _b[1]*_ud;
|
|
_ud = ud;
|
|
|
|
in += vlen;
|
|
out += vlen;
|
|
}
|
|
return noutput_items;
|
|
}
|
|
|
|
} /* namespace digitalhf */
|
|
} /* namespace gr */
|