1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-13 19:50:34 +00:00

Avoid writing in the input buffer

This was uncovered when replacing C-style casts to C++-style casts.

Apply project's indentation style
This commit is contained in:
Carles Fernandez 2017-10-16 20:17:32 +02:00
parent 44edbb3946
commit 8ef49734db
3 changed files with 148 additions and 130 deletions

View File

@ -43,14 +43,15 @@
using google::LogMessage;
notch_sptr make_notch_filter(float pfa, float p_c_factor,
int length_, int n_segments_est, int n_segments_reset)
int length_, int n_segments_est, int n_segments_reset)
{
return notch_sptr(new Notch(pfa, p_c_factor, length_, n_segments_est, n_segments_reset));
}
Notch::Notch(float pfa, float p_c_factor, int length_, int n_segments_est, int n_segments_reset) : gr::block("Notch",
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(gr_complex)))
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(gr_complex)))
{
const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
set_alignment(std::max(1, alignment_multiple));
@ -74,6 +75,7 @@ Notch::Notch(float pfa, float p_c_factor, int length_, int n_segments_est, int n
last_out = gr_complex(0,0);
}
Notch::~Notch()
{
volk_free(c_samples);
@ -81,64 +83,70 @@ Notch::~Notch()
volk_free(power_spect);
}
int Notch::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
int Notch::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int index_out = 0;
float sig2dB = 0.0;
float sig2lin = 0.0;
lv_32fc_t dot_prod_;
gr_complex* in = (gr_complex *) input_items[0];
gr_complex* out = (gr_complex *) output_items[0];
const gr_complex* in = reinterpret_cast<const gr_complex *>(input_items[0]);
gr_complex* out = reinterpret_cast<gr_complex *>(output_items[0]);
gr_complex* in_aux = static_cast<gr_complex *>(volk_malloc(ninput_items[0] * sizeof(gr_complex), volk_get_alignment()));
memcpy(in_aux, in, ninput_items[0] * sizeof(gr_complex));
in++;
arma::cx_fvec signal_segment;
arma::cx_fvec signal_segment_fft;
while((index_out + length_) < noutput_items)
{
if((n_segments < n_segments_est) && (filter_state_ == false))
{
signal_segment = arma::cx_fvec(in, length_, false, false);
signal_segment_fft = arma::fft(signal_segment);
volk_32fc_s32f_power_spectrum_32f(power_spect, signal_segment_fft.memptr(), 1.0, length_);
volk_32f_s32f_calc_spectral_noise_floor_32f(&sig2dB, power_spect, 15.0, length_);
sig2lin = std::pow(10.0, (sig2dB / 10.0)) / ((float) n_deg_fred);
noise_pow_est = (((float) n_segments) * noise_pow_est + sig2lin) / ((float)(n_segments + 1));
memcpy(out, in, sizeof(gr_complex) * length_);
}
else
{
volk_32fc_x2_conjugate_dot_prod_32fc(&dot_prod_, in, in, length_);
if( (lv_creal(dot_prod_) / noise_pow_est) > thres_)
{
if(filter_state_ == false)
if((n_segments < n_segments_est) && (filter_state_ == false))
{
filter_state_ = true;
last_out = gr_complex(0,0);
signal_segment = arma::cx_fvec(in_aux, length_, false, false);
signal_segment_fft = arma::fft(signal_segment);
volk_32fc_s32f_power_spectrum_32f(power_spect, signal_segment_fft.memptr(), 1.0, length_);
volk_32f_s32f_calc_spectral_noise_floor_32f(&sig2dB, power_spect, 15.0, length_);
sig2lin = std::pow(10.0, (sig2dB / 10.0)) / (static_cast<float>(n_deg_fred) );
noise_pow_est = (static_cast<float>(n_segments) * noise_pow_est + sig2lin) / (static_cast<float>(n_segments + 1));
memcpy(out, in, sizeof(gr_complex) * length_);
}
volk_32fc_x2_multiply_conjugate_32fc(c_samples, in, (in - 1), length_);
volk_32fc_s32f_atan2_32f(angle_, c_samples, ((float)1.0), length_);
for(int aux = 0; aux < length_; aux++)
{
z_0 = std::exp(gr_complex(0,1) * (*(angle_ + aux)));
*(out + aux) = *(in + aux) - z_0 * (*(in + aux - 1)) + p_c_factor * z_0 * last_out;
last_out = *(out + aux);
}
}
else
{
if (n_segments > n_segments_reset)
{
n_segments = 0;
volk_32fc_x2_conjugate_dot_prod_32fc(&dot_prod_, in, in, length_);
if( (lv_creal(dot_prod_) / noise_pow_est) > thres_)
{
if(filter_state_ == false)
{
filter_state_ = true;
last_out = gr_complex(0,0);
}
volk_32fc_x2_multiply_conjugate_32fc(c_samples, in, (in - 1), length_);
volk_32fc_s32f_atan2_32f(angle_, c_samples, static_cast<float>(1.0), length_);
for(int aux = 0; aux < length_; aux++)
{
z_0 = std::exp(gr_complex(0,1) * (*(angle_ + aux)));
*(out + aux) = *(in + aux) - z_0 * (*(in + aux - 1)) + p_c_factor * z_0 * last_out;
last_out = *(out + aux);
}
}
else
{
if (n_segments > n_segments_reset)
{
n_segments = 0;
}
filter_state_ = false;
memcpy(out, in, sizeof(gr_complex) * length_);
}
}
filter_state_ = false;
memcpy(out, in, sizeof(gr_complex) * length_);
}
index_out += length_;
n_segments++;
in += length_;
out += length_;
}
index_out += length_;
n_segments++;
in += length_;
out += length_;
}
volk_free(in_aux);
consume_each(index_out);
return index_out;
}

View File

@ -47,9 +47,10 @@ notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int length_,
return notch_lite_sptr(new NotchLite(p_c_factor, pfa, length_, n_segments_est, n_segments_reset, n_segments_coeff));
}
NotchLite::NotchLite(float p_c_factor, float pfa, int length_, int n_segments_est, int n_segments_reset, int n_segments_coeff) : gr::block("NotchLite",
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(gr_complex)))
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(gr_complex)))
{
const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
set_alignment(std::max(1, alignment_multiple));
@ -75,82 +76,88 @@ NotchLite::NotchLite(float p_c_factor, float pfa, int length_, int n_segments_es
angle1 = 0.0;
angle2 = 0.0;
power_spect = static_cast<float *>(volk_malloc(length_ * sizeof(float), volk_get_alignment()));
}
NotchLite::~NotchLite()
{
volk_free(power_spect);
}
int NotchLite::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
int NotchLite::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int index_out = 0;
float sig2dB = 0.0;
float sig2lin = 0.0;
lv_32fc_t dot_prod_;
gr_complex* in = (gr_complex *) input_items[0];
gr_complex* out = (gr_complex *) output_items[0];
const gr_complex* in = reinterpret_cast<const gr_complex *>(input_items[0]);
gr_complex* out = reinterpret_cast<gr_complex *>(output_items[0]);
gr_complex* in_aux = static_cast<gr_complex *>(volk_malloc(ninput_items[0] * sizeof(gr_complex), volk_get_alignment()));
memcpy(in_aux, in, ninput_items[0] * sizeof(gr_complex));
in++;
arma::cx_fvec signal_segment;
arma::cx_fvec signal_segment_fft;
while((index_out + length_) < noutput_items)
{
if((n_segments < n_segments_est) && (filter_state_ == false))
{
signal_segment = arma::cx_fvec(in, length_, false, false);
signal_segment_fft = arma::fft(signal_segment);
volk_32fc_s32f_power_spectrum_32f(power_spect, signal_segment_fft.memptr(), 1.0, length_);
volk_32f_s32f_calc_spectral_noise_floor_32f(&sig2dB, power_spect, 15.0, length_);
sig2lin = std::pow(10.0, (sig2dB / 10.0)) / ((float) n_deg_fred);
noise_pow_est = (((float) n_segments) * noise_pow_est + sig2lin) / ((float)(n_segments + 1));
memcpy(out, in, sizeof(gr_complex) * length_);
}
else
{
volk_32fc_x2_conjugate_dot_prod_32fc(&dot_prod_, in, in, length_);
if( (lv_creal(dot_prod_) / noise_pow_est) > thres_)
{
if(filter_state_ == false)
if((n_segments < n_segments_est) && (filter_state_ == false))
{
filter_state_ = true;
last_out = gr_complex(0,0);
n_segments_coeff = 0;
signal_segment = arma::cx_fvec(in_aux, length_, false, false);
signal_segment_fft = arma::fft(signal_segment);
volk_32fc_s32f_power_spectrum_32f(power_spect, signal_segment_fft.memptr(), 1.0, length_);
volk_32f_s32f_calc_spectral_noise_floor_32f(&sig2dB, power_spect, 15.0, length_);
sig2lin = std::pow(10.0, (sig2dB / 10.0)) / static_cast<float>(n_deg_fred);
noise_pow_est = (static_cast<float>(n_segments) * noise_pow_est + sig2lin) / static_cast<float>(n_segments + 1);
memcpy(out, in, sizeof(gr_complex) * length_);
}
if(n_segments_coeff == 0)
{
volk_32fc_x2_multiply_conjugate_32fc(&c_samples1, (in + 1), in, 1);
volk_32fc_s32f_atan2_32f(&angle1, &c_samples1, ((float)1.0), 1);
volk_32fc_x2_multiply_conjugate_32fc(&c_samples2, (in + length_ - 1), (in + length_ - 2), 1);
volk_32fc_s32f_atan2_32f(&angle2, &c_samples2, ((float)1.0), 1);
float angle_ = (angle1 + angle2) / 2.0;
z_0 = std::exp(gr_complex(0,1) * angle_);
}
for(int aux = 0; aux < length_; aux++)
{
*(out + aux) = *(in + aux) - z_0 * (*(in + aux - 1)) + p_c_factor * z_0 * last_out;
last_out = *(out + aux);
}
n_segments_coeff++;
n_segments_coeff = n_segments_coeff % n_segments_coeff_reset;
}
else
{
if (n_segments > n_segments_reset)
{
n_segments = 0;
volk_32fc_x2_conjugate_dot_prod_32fc(&dot_prod_, in, in, length_);
if( (lv_creal(dot_prod_) / noise_pow_est) > thres_)
{
if(filter_state_ == false)
{
filter_state_ = true;
last_out = gr_complex(0,0);
n_segments_coeff = 0;
}
if(n_segments_coeff == 0)
{
volk_32fc_x2_multiply_conjugate_32fc(&c_samples1, (in + 1), in, 1);
volk_32fc_s32f_atan2_32f(&angle1, &c_samples1, static_cast<float>(1.0), 1);
volk_32fc_x2_multiply_conjugate_32fc(&c_samples2, (in + length_ - 1), (in + length_ - 2), 1);
volk_32fc_s32f_atan2_32f(&angle2, &c_samples2, static_cast<float>(1.0), 1);
float angle_ = (angle1 + angle2) / 2.0;
z_0 = std::exp(gr_complex(0,1) * angle_);
}
for(int aux = 0; aux < length_; aux++)
{
*(out + aux) = *(in + aux) - z_0 * (*(in + aux - 1)) + p_c_factor * z_0 * last_out;
last_out = *(out + aux);
}
n_segments_coeff++;
n_segments_coeff = n_segments_coeff % n_segments_coeff_reset;
}
else
{
if (n_segments > n_segments_reset)
{
n_segments = 0;
}
filter_state_ = false;
memcpy(out, in, sizeof(gr_complex) * length_);
}
}
filter_state_ = false;
memcpy(out, in, sizeof(gr_complex) * length_);
}
index_out += length_;
n_segments++;
in += length_;
out += length_;
}
index_out += length_;
n_segments++;
in += length_;
out += length_;
}
volk_free(in_aux);
consume_each(index_out);
return index_out;
}

View File

@ -39,14 +39,15 @@
using google::LogMessage;
pulse_blanking_cc_sptr make_pulse_blanking_cc(float pfa, int length_,
int n_segments_est, int n_segments_reset)
int n_segments_est, int n_segments_reset)
{
return pulse_blanking_cc_sptr(new pulse_blanking_cc(pfa, length_, n_segments_est, n_segments_reset));
}
pulse_blanking_cc::pulse_blanking_cc(float pfa, int length_, int n_segments_est, int n_segments_reset) : gr::block("pulse_blanking_cc",
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(gr_complex)))
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(gr_complex)))
{
const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
set_alignment(std::max(1, alignment_multiple));
@ -63,55 +64,57 @@ pulse_blanking_cc::pulse_blanking_cc(float pfa, int length_, int n_segments_est,
thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa));
zeros_ = static_cast<gr_complex *>(volk_malloc(length_ * sizeof(gr_complex), volk_get_alignment()));
for (int aux = 0; aux < length_; aux++)
{
zeros_[aux] = gr_complex(0, 0);
}
{
zeros_[aux] = gr_complex(0, 0);
}
}
pulse_blanking_cc::~pulse_blanking_cc()
{
volk_free(zeros_);
}
int pulse_blanking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
gr_complex *in = (gr_complex *) input_items[0];
gr_complex *out = (gr_complex *) output_items[0];
const gr_complex* in = reinterpret_cast<const gr_complex *>(input_items[0]);
gr_complex* out = reinterpret_cast<gr_complex *>(output_items[0]);
float* magnitude = static_cast<float *>(volk_malloc(noutput_items * sizeof(float), volk_get_alignment()));
volk_32fc_magnitude_squared_32f(magnitude, in, noutput_items);
int sample_index = 0;
float segment_energy;
while((sample_index + length_) < noutput_items)
{
volk_32f_accumulator_s32f(&segment_energy, (magnitude + sample_index), length_);
if((n_segments < n_segments_est) && (last_filtered == false))
{
noise_power_estimation = (((float) n_segments) * noise_power_estimation + segment_energy / ((float)n_deg_fred)) / ((float)(n_segments + 1));
memcpy(out, in, sizeof(gr_complex)*length_);
}
else
{
if((segment_energy/noise_power_estimation) > thres_)
{
memcpy(out, zeros_, sizeof(gr_complex)*length_);
last_filtered = true;
}
else
{
memcpy(out, in, sizeof(gr_complex)*length_);
last_filtered = false;
if (n_segments > n_segments_reset)
volk_32f_accumulator_s32f(&segment_energy, (magnitude + sample_index), length_);
if((n_segments < n_segments_est) && (last_filtered == false))
{
n_segments = 0;
noise_power_estimation = (((float) n_segments) * noise_power_estimation + segment_energy / ((float)n_deg_fred)) / ((float)(n_segments + 1));
memcpy(out, in, sizeof(gr_complex)*length_);
}
}
else
{
if((segment_energy/noise_power_estimation) > thres_)
{
memcpy(out, zeros_, sizeof(gr_complex)*length_);
last_filtered = true;
}
else
{
memcpy(out, in, sizeof(gr_complex)*length_);
last_filtered = false;
if (n_segments > n_segments_reset)
{
n_segments = 0;
}
}
}
in+=length_;
out+=length_;
sample_index+=length_;
n_segments++;
}
in+=length_;
out+=length_;
sample_index+=length_;
n_segments++;
}
volk_free(magnitude);
consume_each(sample_index);
return sample_index;