Beta version of notch input filter

Beta version of general_work method implemented

src/algorithms/input_filter/adapters/notch_filter.cc

@@ -37,7 +37,7 @@
 #include <gnuradio/blocks/file_sink.h>
 #include <glog/logging.h>
 #include "configuration_interface.h"
-#include "notch_cc.cc"
+#include "notch_cc.h"
 
 using google::LogMessage;
 
@@ -47,12 +47,12 @@ NotchFilter::NotchFilter(ConfigurationInterface* configuration, std::string role
                 out_streams_(out_streams)
 {
     size_t item_size_;
-    double pfa;
-    double default_pfa = 0.001;
-    double p_c_factor;
-    double default_p_c_factor = 0.9;
-    unsigned int length_;
-    unsigned int default_length_ = 5;
+    float pfa;
+    float default_pfa = 0.001;
+    float p_c_factor;
+    float default_p_c_factor = 0.9;
+    int length_;
+    int default_length_ = 5;
     std::string default_item_type = "gr_complex";
     std::string default_dump_file = "./data/input_filter.dat";
     item_type_ = configuration->property(role + ".item_type", default_item_type);

src/algorithms/input_filter/adapters/notch_filter.h

@@ -37,6 +37,7 @@
 #include <vector>
 #include <gnuradio/blocks/file_sink.h>
 #include "gnss_block_interface.h"
+#include "notch_cc.h"
 
 
 class ConfigurationInterface;
@@ -77,7 +78,7 @@ private:
     unsigned int in_streams_;
     unsigned int out_streams_;
     gr::blocks::file_sink::sptr file_sink_;
-    gr::block_sptr notch_filter_;
+    notch_sptr notch_filter_;
 };
 
 #endif //GNSS_SDR_NOTCH_FILTER_H_

src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc

@@ -32,27 +32,32 @@
 #include <boost/math/distributions/chi_squared.hpp>
 #include <cmath>
 #include <complex>
-#include <gnuradio/block.h>
 #include <gnuradio/io_signature.h>
+#include <volk/volk.h>
 
-notch_sptr make_notch_filter(double pfa, double p_c_factor, 
-                             unsigned int length_)
+notch_sptr make_notch_filter(float pfa, float p_c_factor, 
+                             int length_)
 {
     return notch_sptr(new Notch(pfa, p_c_factor, length_));
 }
 
-Notch::Notch(double pfa, double p_c_factor, unsigned int length_) : gr::block("Notch",
+Notch::Notch(float pfa, float p_c_factor, int length_) : gr::block("Notch",
                                                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));
     this->pfa = pfa;
-    this->noise_pow_est = 0.0;
+    noise_pow_est = 0.0;
     this->p_c_factor = p_c_factor;
     this->length_ = length_;
-    filter_state_ = 0;
+    filter_state_ = false;
     n_deg_fred = 2 * length_;
+    n_segments_est = 5;
+    n_segments = 0;
     z_0 = gr_complex(0 , 0);
-    thres_ = 
+    boost::math::chi_squared_distribution<float> my_dist_(n_deg_fred);
+    thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa));
     
 }
 
@@ -60,5 +65,59 @@ Notch::Notch(double pfa, double p_c_factor, unsigned int length_) : gr::block("N
 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)
 {
-    
+    gr_complex * in = (gr_complex *) input_items[0];
+    gr_complex *out = (gr_complex *) output_items[0];
+    gr_complex * paux;
+    int samples_proc = 0;
+    int aux = 0;
+    gr_complex magnitude;
+    float sig2 = 0.0;
+    float* angle_;
+    gr_complex * c_samples;
+    c_samples = static_cast<gr_complex *>(volk_malloc(length_ * sizeof(gr_complex), volk_get_alignment()));
+    angle_ = static_cast<float *>(volk_malloc(length_ * sizeof(float), volk_get_alignment()));
+    while(((samples_proc + length_) < noutput_items) && (n_segments < n_segments_est))
+    {
+        volk_32fc_x2_conjugate_dot_prod_32fc(&magnitude, in, in, length_);
+        sig2 = magnitude.real() / ((float) n_deg_fred);
+        noise_pow_est = (((float) n_segments) * noise_pow_est + sig2) / ((float)(n_segments + 1));
+        samples_proc = samples_proc + length_;
+        n_segments++;
+        memcpy(out, in, sizeof(gr_complex)*length_);
+        in = (gr_complex *) input_items[samples_proc];
+        out = (gr_complex *) output_items[samples_proc];
+    }
+    while((samples_proc + length_) < noutput_items)
+    {
+        volk_32fc_x2_conjugate_dot_prod_32fc(&magnitude, in, in, length_);
+        if( (magnitude.real() / noise_pow_est) > thres_)
+        {
+            filter_state_ = true;
+            paux = (gr_complex *) input_items[samples_proc-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[samples_proc] = in[samples_proc] - z_0 * in[samples_proc - 1]
+                                    + gr_complex(p_c_factor,0) * z_0 * out[samples_proc -1];
+                samples_proc++;
+                in = (gr_complex *) input_items[samples_proc];
+                out = (gr_complex *) output_items[samples_proc];
+            }
+            
+        }
+        else
+        {
+            filter_state_ = false;
+            samples_proc = samples_proc + length_;
+            memcpy(out, in, sizeof(gr_complex)*length_);
+            in = (gr_complex *) input_items[samples_proc];
+            out = (gr_complex *) output_items[samples_proc];
+        }
+    }
+    volk_free(c_samples);
+    volk_free(angle_);
+    consume_each(samples_proc);
+    return samples_proc;
 }

src/algorithms/input_filter/gnuradio_blocks/notch_cc.h

@@ -38,37 +38,41 @@ class Notch;
 
 typedef boost::shared_ptr<Notch> notch_sptr;
 
-notch_sptr make_notch_filter(double pfa, double p_c_factor, 
-                             unsigned int length_);
+notch_sptr make_notch_filter(float pfa, float p_c_factor, 
+                             int length_);
 
 /*!
  * \brief This class implements a real-time software-defined multi state notch filter
  */
 
-class Notch: public gr::block
+class Notch : public gr::block
 {
 private:
     
-    friend notch_sptr make_notch_filter(double pfa, double p_c_factor,
-                                        unsigned int length_);
-    double pfa;
-    double noise_pow_est;
-    double p_c_factor;
-    double thres_;
-    unsigned int length_;
-    unsigned int n_deg_fred;
-    unsigned int filter_state_;
+    float pfa;
+    float noise_pow_est;
+    float p_c_factor;
+    float thres_;
+    int length_;
+    int n_segments_est;
+    int n_segments;
+    int n_deg_fred;
+    bool filter_state_;
     gr_complex z_0;
     
     
 public:
     
-    Notch(double pfa, double p_c_factor, unsigned int length_);
+    //friend notch_sptr make_notch_filter(float pfa, float p_c_factor,
+    //                                    int length_);
+    
+    Notch(float pfa, float p_c_factor, int length_);
     
     ~Notch();
     
-    int work (int noutput_items, gr_vector_const_void_star &input_items,
-              gr_vector_void_star &output_items);
+    int general_work (int noutput_items, gr_vector_int &ninput_items, 
+                      gr_vector_const_void_star &input_items,
+                      gr_vector_void_star &output_items);
 };
 
 #endif //GNSS_SDR_NOTCH_H_