207 lines
7.7 KiB
C++
207 lines
7.7 KiB
C++
/* -*- c++ -*- */
|
|
/*
|
|
* Copyright 2020
|
|
* Federico "Larroca" La Rocca <flarroca@fing.edu.uy>
|
|
*
|
|
* Instituto de Ingenieria Electrica, Facultad de Ingenieria,
|
|
* Universidad de la Republica, Uruguay.
|
|
*
|
|
* 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 <gnuradio/io_signature.h>
|
|
#include "sampling_synchronization_impl.h"
|
|
#include <volk/volk.h>
|
|
#include <random>
|
|
|
|
namespace gr {
|
|
namespace tempest {
|
|
|
|
sampling_synchronization::sptr
|
|
sampling_synchronization::make(int Htotal, double manual_correction)
|
|
{
|
|
return gnuradio::get_initial_sptr
|
|
(new sampling_synchronization_impl(Htotal, manual_correction));
|
|
}
|
|
|
|
|
|
/*
|
|
* The private constructor
|
|
*/
|
|
sampling_synchronization_impl::sampling_synchronization_impl(int Htotal, double manual_correction)
|
|
: gr::block("sampling_synchronization",
|
|
gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
|
gr::io_signature::make(1, 1, sizeof(gr_complex))),
|
|
d_inter(gr::filter::mmse_fir_interpolator_cc()),
|
|
d_dist(0, 1),
|
|
d_gen(std::random_device{}())
|
|
|
|
{
|
|
set_relative_rate(1);
|
|
d_Htotal = Htotal;
|
|
|
|
//VOLK alignment as recommended by GNU Radio's Manual. It has a similar effect
|
|
//than set_output_multiple(), thus we will generally get multiples of this value
|
|
//as noutput_items.
|
|
const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
|
|
set_alignment(std::max(1, alignment_multiple));
|
|
|
|
d_max_deviation = 0.10;
|
|
d_max_deviation_px = (int)std::ceil(Htotal*d_max_deviation);
|
|
d_samp_inc_remainder = 0;
|
|
d_samp_inc_correction = manual_correction;
|
|
d_samp_phase = 0;
|
|
d_alpha_samp_inc = 1e-3;
|
|
|
|
|
|
d_current_corr = new gr_complex[2*d_max_deviation_px + 1];
|
|
d_historic_corr = new gr_complex[2*d_max_deviation_px + 1];
|
|
d_abs_historic_corr = new float[2*d_max_deviation_px + 1];
|
|
for (int i = 0; i<2*d_max_deviation_px+1; i++){
|
|
d_historic_corr[i] = 0;
|
|
d_abs_historic_corr[i] = 0;
|
|
}
|
|
d_alpha_corr = 1e-6;
|
|
|
|
d_proba_of_updating = 0.01;
|
|
|
|
}
|
|
|
|
/*
|
|
* Our virtual destructor.
|
|
*/
|
|
sampling_synchronization_impl::~sampling_synchronization_impl()
|
|
{
|
|
delete [] d_current_corr;
|
|
delete [] d_historic_corr;
|
|
delete [] d_abs_historic_corr;
|
|
}
|
|
|
|
void
|
|
sampling_synchronization_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required)
|
|
{
|
|
int ninputs = ninput_items_required.size ();
|
|
// make sure we receive at least Hsize+max_deviation+taps_to_interpolate
|
|
for (int i = 0; i < ninputs; i++)
|
|
{
|
|
//ninput_items_required[i] = ( d_cp_length + d_fft_length ) * (noutput_items + 1) ;
|
|
ninput_items_required[i] = (int)ceil((noutput_items + 1) * (1+d_samp_inc_remainder)) + d_inter.ntaps() + d_Htotal+d_max_deviation_px ;
|
|
}
|
|
}
|
|
|
|
void sampling_synchronization_impl::set_Htotal(int Htotal){
|
|
d_Htotal = Htotal;
|
|
d_max_deviation_px = (int)std::ceil(Htotal*d_max_deviation);
|
|
printf("[TEMPEST] Setting Htotal to %i in sampling synchronization block.\n", Htotal);
|
|
}
|
|
|
|
void sampling_synchronization_impl::set_manual_correction(double correction){
|
|
|
|
d_samp_inc_correction = correction;
|
|
|
|
}
|
|
|
|
void sampling_synchronization_impl::update_interpolation_ratio(const float * datain, const int datain_length){
|
|
uint16_t peak_index = 0;
|
|
uint32_t d_datain_length = (uint32_t)(datain_length);
|
|
volk_32f_index_max_16u(&peak_index, datain, datain_length);
|
|
|
|
// the new interpolation ratio is how far the peak is from d_Htotal.
|
|
d_new_interpolation_ratio = ((float)(peak_index-d_max_deviation_px + d_Htotal))/(float)d_Htotal;
|
|
//for (int i=0; i<datain_length; i++)
|
|
// printf("datain[%i]=%f\n", i, datain[i]);
|
|
//printf("peak_index: %i\n",peak_index);
|
|
//printf("new_interpolation_ratio: %f\n",d_new_interpolation_ratio);
|
|
//printf("d_samp_inc: %.20f\n",d_samp_inc_remainder+1);
|
|
|
|
//d_samp_inc = (1-d_alpha_samp_inc)*d_samp_inc - d_alpha_samp_inc* d_new_interpolation_ratio;
|
|
d_samp_inc_remainder = d_samp_inc_remainder - d_alpha_samp_inc*(d_samp_inc_remainder+1 - d_new_interpolation_ratio);
|
|
|
|
}
|
|
|
|
int sampling_synchronization_impl::interpolate_input(const gr_complex * in, gr_complex * out, int size){
|
|
int ii = 0; // input index
|
|
int oo = 0; // output index
|
|
|
|
double s, f;
|
|
int incr;
|
|
while(oo < size) {
|
|
out[oo++] = d_inter.interpolate(&in[ii], d_samp_phase);
|
|
|
|
s = d_samp_phase + d_samp_inc_remainder + d_samp_inc_correction + 1;
|
|
f = floor(s);
|
|
incr = (int)f;
|
|
d_samp_phase = s - f;
|
|
ii += incr;
|
|
}
|
|
|
|
// return how many inputs we required to generate d_cp_length+d_fft_length outputs
|
|
return ii;
|
|
}
|
|
|
|
int
|
|
sampling_synchronization_impl::general_work (int noutput_items,
|
|
gr_vector_int &ninput_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];
|
|
|
|
d_in_conj = new gr_complex[noutput_items];
|
|
volk_32fc_conjugate_32fc(&d_in_conj[0], &in[0], noutput_items);
|
|
|
|
if(d_dist(d_gen)<d_proba_of_updating){
|
|
for (int i=0; i<noutput_items; i++){
|
|
// I calculate the correlation between the current sample and the past samples,
|
|
// and update the historic values
|
|
volk_32fc_s32fc_multiply_32fc(&d_current_corr[0], &in[i+d_Htotal-d_max_deviation_px], d_in_conj[i]*d_alpha_corr, 2*d_max_deviation_px+1);
|
|
|
|
volk_32fc_s32fc_multiply_32fc(&d_historic_corr[0], &d_historic_corr[0], (1-d_alpha_corr), 2*d_max_deviation_px+1);
|
|
volk_32fc_x2_add_32fc(&d_historic_corr[0], &d_historic_corr[0], &d_current_corr[0], 2*d_max_deviation_px+1);
|
|
volk_32fc_magnitude_squared_32f(&d_abs_historic_corr[0], &d_historic_corr[0], 2*d_max_deviation_px+1);
|
|
|
|
//for (int dev=0; dev<2*d_max_deviation_px+1; dev++)
|
|
// printf("d_current_corr[%i]=%f+j %f\n", dev, std::real(d_current_corr[dev]), std::imag(d_current_corr[dev]));
|
|
update_interpolation_ratio(d_abs_historic_corr, 2*d_max_deviation_px+1);
|
|
}
|
|
}
|
|
|
|
//printf("d_samp_inc: %f\n", d_samp_inc);
|
|
|
|
int required_for_interpolation = 0;
|
|
required_for_interpolation = interpolate_input(&in[0], &out[0],noutput_items);
|
|
|
|
// Tell runtime system how many input items we consumed on
|
|
// each input stream.
|
|
consume_each (required_for_interpolation);
|
|
|
|
// Tell runtime system how many output items we produced.
|
|
return noutput_items;
|
|
|
|
|
|
}
|
|
|
|
} /* namespace tempest */
|
|
} /* namespace gr */
|
|
|