diff --git a/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc b/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc index cedad7741..fed7205cc 100644 --- a/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc +++ b/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc @@ -52,13 +52,13 @@ PulseBlankingFilter::PulseBlankingFilter(ConfigurationInterface* configuration, output_item_type_ = config_->property(role_ + ".output_item_type", default_output_item_type); dump_ = config_->property(role_ + ".dump", false); dump_filename_ = config_->property(role_ + ".dump_filename", default_dump_filename); - float default_pfa_ = 0.01; + float default_pfa_ = 0.04; float pfa = config_->property(role_ + ".pfa", default_pfa_); - int default_length_ = 16; + int default_length_ = 32; int length_ = config_->property(role_ + ".length", default_length_); - int default_n_segments_est = 25000; + int default_n_segments_est = 12500; int n_segments_est = config_->property(role_ + ".segments_estimation", default_n_segments_est); - int default_n_segments_reset = 500000; + int default_n_segments_reset = 5000000; int n_segments_reset = config_->property(role_ + ".segments_reset", default_n_segments_reset); if (input_item_type_.compare("gr_complex") == 0) { diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc index 8c79d3b50..007b3d9b3 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc @@ -84,75 +84,61 @@ Notch::~Notch() int Notch::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) { - - int index_in = 1; int index_out = 0; - int aux = 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]; - gr_complex* paux; in++; arma::cx_fvec signal_segment; arma::cx_fvec signal_segment_fft; - while(((index_out + length_) < noutput_items) && (n_segments < n_segments_est) && (filter_state_ == false)) + while((index_out + length_) < noutput_items) { - 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_); + 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) + { + 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, ((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_); + } + } index_out += length_; - index_in += length_; n_segments++; in += length_; out += length_; } - while((index_out + length_) < noutput_items) - { - n_segments++; - 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); - } - paux = in - 1; - volk_32fc_x2_multiply_conjugate_32fc(c_samples, in, paux, length_); - volk_32fc_s32f_atan2_32f(angle_, c_samples, ((float)1.0), length_); - for(aux = 0; aux < length_; aux++) - { - z_0 = std::exp(gr_complex(0,1) * (*(angle_ + aux))); - *out = (*in) - z_0 * (*(in - 1)) - + p_c_factor * z_0 * last_out; - last_out = *out; - index_out++; - index_in++; - in ++; - out ++; - } - - } - else - { - if (n_segments > n_segments_reset) - { - n_segments = 0; - } - filter_state_ = false; - memcpy(out, in, sizeof(gr_complex) * length_); - index_out += length_; - index_in += length_; - in += length_; - out += length_; - } - } consume_each(index_out); return index_out; } diff --git a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc index ec758e511..a744f8747 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc @@ -44,8 +44,6 @@ pulse_blanking_cc_sptr make_pulse_blanking_cc(float pfa, int length_, 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))) @@ -64,7 +62,6 @@ pulse_blanking_cc::pulse_blanking_cc(float pfa, int length_, int n_segments_est, boost::math::chi_squared_distribution my_dist_(n_deg_fred); thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa)); zeros_ = static_cast(volk_malloc(length_ * sizeof(gr_complex), volk_get_alignment())); - magnitude = static_cast(volk_malloc(length_ * sizeof(float), volk_get_alignment())); for (int aux = 0; aux < length_; aux++) { zeros_[aux] = gr_complex(0, 0); @@ -73,8 +70,7 @@ pulse_blanking_cc::pulse_blanking_cc(float pfa, int length_, int n_segments_est, pulse_blanking_cc::~pulse_blanking_cc() { - volk_free(zeros_); - volk_free(magnitude); + volk_free(zeros_); } int pulse_blanking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), @@ -82,12 +78,13 @@ int pulse_blanking_cc::general_work (int noutput_items __attribute__((unused)), { gr_complex *in = (gr_complex *) input_items[0]; gr_complex *out = (gr_complex *) output_items[0]; + float* magnitude = static_cast(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_32fc_magnitude_squared_32f(magnitude, in, length_); - volk_32f_accumulator_s32f(&segment_energy, magnitude, length_); + 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)); @@ -115,6 +112,7 @@ int pulse_blanking_cc::general_work (int noutput_items __attribute__((unused)), sample_index+=length_; n_segments++; } + volk_free(magnitude); consume_each(sample_index); return sample_index; } diff --git a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h index 1bb9a7550..a77cf4deb 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h @@ -54,7 +54,6 @@ private: float noise_power_estimation; float thres_; float pfa; - float* magnitude; gr_complex* zeros_; public: diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index cac56293a..6efe76db5 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -66,6 +66,7 @@ #include "beamformer_filter.h" #include "pulse_blanking_filter.h" #include "notch_filter.h" +#include "notch_filter_lite.h" #include "gps_l1_ca_pcps_acquisition.h" #include "gps_l2_m_pcps_acquisition.h" #include "gps_l1_ca_pcps_multithread_acquisition.h" @@ -889,6 +890,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("Notch_Filter_Lite") == 0) + { + std::unique_ptr block_(new NotchFilterLite(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } // RESAMPLER -------------------------------------------------------------------