deep-tempest/gr-tempest/lib/ssamp_correction_impl.cc

275 lines
9.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 <volk/volk.h>
#include <random>
#include "ssamp_correction_impl.h"
namespace gr {
namespace tempest {
ssamp_correction::sptr
ssamp_correction::make(int Htotal, int Vtotal, int correct_sampling, float max_deviation)
{
return gnuradio::get_initial_sptr
(new ssamp_correction_impl(Htotal, Vtotal, correct_sampling, max_deviation));
}
/*
* The private constructor
*/
ssamp_correction_impl::ssamp_correction_impl(int Htotal, int Vtotal, int correct_sampling, float max_deviation)
: gr::block("ssamp_correction",
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_gen(std::random_device{}())
{
set_relative_rate(1);
d_correct_sampling = correct_sampling;
d_max_deviation = max_deviation;
d_Htotal = Htotal;
d_Vtotal = Vtotal;
d_max_deviation_px = (int)std::ceil(d_Htotal*d_max_deviation);
set_history(d_Vtotal*(d_Htotal+d_max_deviation_px)+1);
d_peak_line_index = 0;
d_samp_inc_rem = 0;
d_stop_fine_sampling_synch = 0;
d_new_interpolation_ratio_rem = 0;
d_current_line_corr = new gr_complex[2*d_max_deviation_px + 1];
d_historic_line_corr = new gr_complex[2*d_max_deviation_px + 1];
d_abs_historic_line_corr = new float[2*d_max_deviation_px + 1];
/** Note: d_current_frame_corr[i] and derivatives will keep the correlation between pixels
px[t] and px[t+Htotal*Vtotal+i]. Since a single pixel de-alignment with the next line will
mean d_Vtotal pixels de-alignments with the next frame, these arrays are much bigger.
However, instead of always calculating the whole of them, I'll only calculate around those
indicated by the max in the d_abs_historic_line_corr.
*/
d_current_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
d_historic_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
d_abs_historic_frame_corr = new float[2*(d_max_deviation_px+1)*d_Vtotal + 1];
d_alpha_samp_inc = 1e-1;
d_samp_phase = 0;
d_alpha_corr = 1e-2;
d_next_update = 0;
for (int i = 0; i<2*d_max_deviation_px+1; i++){
d_historic_line_corr[i] = 0;
d_abs_historic_line_corr[i] = 0;
}
for (int i = 0; i<2*d_max_deviation_px*d_Vtotal+1; i++){
d_historic_frame_corr[i] = 0;
d_abs_historic_frame_corr[i] = 0;
}
printf("[TEMPEST] Construction of ssamp_correction_impl with Htotal=%i and Vtotal=%i.\n", Htotal, Vtotal);
//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));
// PMT ports
message_port_register_in(pmt::mp("ratio"));
message_port_register_in(pmt::mp("en"));
// PMT handlers
// : Port Lambda function
set_msg_handler( pmt::mp("ratio"), [this](const pmt::pmt_t& msg) {ssamp_correction_impl::set_ratio_msg(msg); });
set_msg_handler( pmt::mp("en"), [this](const pmt::pmt_t& msg) {ssamp_correction_impl::set_ena_msg(msg); });
}
void
ssamp_correction_impl::set_ena_msg(pmt::pmt_t msg)
{
if (pmt::is_bool(msg)) {
bool en = pmt::to_bool(msg);
gr::thread::scoped_lock l(d_mutex);
d_stop_fine_sampling_synch = !en;
printf("ssamp_correction_impl Received Sampling Stop.\n");
} else {
GR_LOG_WARN(d_logger,
"ssamp_correction_impl Received : Non-PMT type received, expecting Boolean PMT\n");
}
}
void
ssamp_correction_impl::set_ratio_msg(pmt::pmt_t msg)
{
if(pmt::is_pair(msg)) {
// saca el primero de la pareja
pmt::pmt_t key = pmt::car(msg);
// saca el segundo
pmt::pmt_t val = pmt::cdr(msg);
if(pmt::eq(key, pmt::string_to_symbol("ratio"))) {
if(pmt::is_number(val)) {
d_new_interpolation_ratio_rem = (double)pmt::to_double(val);
printf("ssamp_correction_impl Received : interpolation ratio = %f \n", d_new_interpolation_ratio_rem);
}
}
}
}
/*
* Our virtual destructor.
*/
ssamp_correction_impl::~ssamp_correction_impl()
{
delete [] d_current_line_corr;
delete [] d_historic_line_corr;
delete [] d_abs_historic_line_corr;
delete [] d_current_frame_corr;
delete [] d_historic_frame_corr;
delete [] d_abs_historic_frame_corr;
}
void
ssamp_correction_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] = (int)ceil((noutput_items + 1) * (2+d_samp_inc_rem)) + d_inter.ntaps() ;
}
}
void
ssamp_correction_impl::set_Htotal_Vtotal(int Htotal, int Vtotal){
// If the resolution's changed, I reset the whole block
/**
* @fcarraustewart
* FIXME: This callback double taps onChange
* of any of these two variables, Htotal Vtotal,
* This happens on all our testXXX.grc files.
* Solution .grc file should not have a link
* between these two variables?
*/
d_Htotal = Htotal;
d_Vtotal = Vtotal;
d_max_deviation_px = (int)std::ceil(d_Htotal*d_max_deviation);
printf("ssamp_correction_impl d_max_deviation_px: %i\n", d_max_deviation_px);
set_history(d_Vtotal*(d_Htotal+d_max_deviation_px)+1);
d_peak_line_index = 0;
d_samp_inc_rem = 0;
d_stop_fine_sampling_synch = 0;
d_new_interpolation_ratio_rem = 0;
/**
* < Calling delete for each of the following pointers
* and assigning them nullptr right here. Before running the new operator.
*
* */
delete [] d_current_line_corr;
delete [] d_historic_line_corr;
delete [] d_abs_historic_line_corr;
delete [] d_current_frame_corr;
delete [] d_historic_frame_corr;
delete [] d_abs_historic_frame_corr;
d_current_line_corr = new gr_complex[2*d_max_deviation_px + 1];
d_historic_line_corr = new gr_complex[2*d_max_deviation_px + 1];
d_abs_historic_line_corr = new float[2*d_max_deviation_px + 1];
d_current_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
d_historic_frame_corr = new gr_complex[2*(d_max_deviation_px+1)*d_Vtotal + 1];
d_abs_historic_frame_corr = new float[2*(d_max_deviation_px+1)*d_Vtotal + 1];
//I'll estimate the new sampling synchronization asap
d_next_update = 0;
for (int i = 0; i<2*d_max_deviation_px+1; i++){
d_historic_line_corr[i] = 0;
d_abs_historic_line_corr[i] = 0;
}
for (int i = 0; i<2*d_max_deviation_px*d_Vtotal+1; i++){
d_historic_frame_corr[i] = 0;
d_abs_historic_frame_corr[i] = 0;
}
printf("[TEMPEST] Construction of ssamp_correction_impl with Htotal=%i and Vtotal=%i.\n", Htotal, Vtotal);
}
int
ssamp_correction_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_rem + 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
ssamp_correction_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];
gr::thread::scoped_lock l(d_mutex);
int required_for_interpolation = noutput_items;
d_samp_inc_rem = d_new_interpolation_ratio_rem;
required_for_interpolation = interpolate_input(&in[0], &out[0], noutput_items);
consume_each (required_for_interpolation);
return noutput_items;
}
} /* namespace tempest */
} /* namespace gr */