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:
parent
44edbb3946
commit
8ef49734db
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user