git.gag.com Git - debian/gnuradio/blob - gnuradio-core/src/lib/filter/gr_fft_filter_fff.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2005 Free Software Foundation, Inc.
   4  *
   5  * This file is part of GNU Radio
   6  *
   7  * GNU Radio is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 3, or (at your option)
  10  * any later version.
  11  *
  12  * GNU Radio is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with GNU Radio; see the file COPYING.  If not, write to
  19  * the Free Software Foundation, Inc., 51 Franklin Street,
  20  * Boston, MA 02110-1301, USA.
  21  */
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include <gr_fft_filter_fff.h>
  28 #include <gr_io_signature.h>
  29 #include <gri_fft.h>
  30 #include <math.h>
  31 #include <assert.h>
  32 #include <stdexcept>
  33 #include <gr_firdes.h>
  34
  35
  36 #include <cstdio>
  37 #include <iostream>
  38 #include <string.h>
  39
  40 gr_fft_filter_fff_sptr gr_make_fft_filter_fff (int decimation, const std::vector<float> &taps)
  41 {
  42   return gr_fft_filter_fff_sptr (new gr_fft_filter_fff (decimation, taps));
  43 }
  44
  45
  46 gr_fft_filter_fff::gr_fft_filter_fff (int decimation, const std::vector<float> &taps)
  47   : gr_sync_decimator ("fft_filter_fff",
  48                        gr_make_io_signature (1, 1, sizeof (float)),
  49                        gr_make_io_signature (1, 1, sizeof (float)),
  50                        decimation),
  51     d_fftsize(-1), d_fwdfft(0), d_invfft(0), d_updated(false)
  52 {
  53   set_history(1);
  54   actual_set_taps(taps);
  55 }
  56
  57 gr_fft_filter_fff::~gr_fft_filter_fff ()
  58 {
  59   delete d_fwdfft;
  60   delete d_invfft;
  61 }
  62
  63 #if 0
  64 static void
  65 print_vector_complex(const std::string label, const std::vector<gr_complex> &x)
  66 {
  67   std::cout << label;
  68   for (unsigned i = 0; i < x.size(); i++)
  69     std::cout << x[i] << " ";
  70   std::cout << "\n";
  71 }
  72
  73 static void
  74 print_vector_float(const std::string label, const std::vector<float> &x)
  75 {
  76   std::cout << label;
  77   for (unsigned i = 0; i < x.size(); i++)
  78     std::cout << x[i] << " ";
  79   std::cout << "\n";
  80 }
  81 #endif
  82
  83 void
  84 gr_fft_filter_fff::set_taps (const std::vector<float> &taps)
  85 {
  86   d_new_taps = taps;
  87   d_updated = true;
  88 }
  89
  90 /*
  91  * determines d_ntaps, d_nsamples, d_fftsize, d_xformed_taps
  92  */
  93 void
  94 gr_fft_filter_fff::actual_set_taps (const std::vector<float> &taps)
  95 {
  96   int i = 0;
  97   compute_sizes(taps.size());
  98
  99   d_tail.resize(tailsize());
 100   for (i = 0; i < tailsize(); i++)
 101     d_tail[i] = 0;
 102
 103   float *in = d_fwdfft->get_inbuf();
 104   gr_complex *out = d_fwdfft->get_outbuf();
 105
 106   float scale = 1.0 / d_fftsize;
 107
 108   // Compute forward xform of taps.
 109   // Copy taps into first ntaps slots, then pad with zeros
 110   for (i = 0; i < d_ntaps; i++)
 111     in[i] = taps[i] * scale;
 112
 113   for (; i < d_fftsize; i++)
 114     in[i] = 0;
 115
 116   d_fwdfft->execute();          // do the xform
 117
 118   // now copy output to d_xformed_taps
 119   for (i = 0; i < d_fftsize/2+1; i++)
 120     d_xformed_taps[i] = out[i];
 121
 122   //print_vector_complex("transformed taps:", d_xformed_taps);
 123 }
 124
 125 // determine and set d_ntaps, d_nsamples, d_fftsize
 126
 127 void
 128 gr_fft_filter_fff::compute_sizes(int ntaps)
 129 {
 130   int old_fftsize = d_fftsize;
 131   d_ntaps = ntaps;
 132   d_fftsize = (int) (2 * pow(2.0, ceil(log(ntaps) / log(2))));
 133   d_nsamples = d_fftsize - d_ntaps + 1;
 134
 135   if (0)
 136     fprintf(stderr, "gr_fft_filter: ntaps = %d, fftsize = %d, nsamples = %d\n",
 137             d_ntaps, d_fftsize, d_nsamples);
 138
 139   assert(d_fftsize == d_ntaps + d_nsamples -1 );
 140
 141   if (d_fftsize != old_fftsize){        // compute new plans
 142     delete d_fwdfft;
 143     delete d_invfft;
 144     d_fwdfft = new gri_fft_real_fwd(d_fftsize);
 145     d_invfft = new gri_fft_real_rev(d_fftsize);
 146     d_xformed_taps.resize(d_fftsize/2+1);
 147   }
 148
 149   set_output_multiple(d_nsamples);
 150 }
 151
 152 int
 153 gr_fft_filter_fff::work (int noutput_items,
 154                          gr_vector_const_void_star &input_items,
 155                          gr_vector_void_star &output_items)
 156 {
 157   const float *in = (const float *) input_items[0];
 158   float *out = (float *) output_items[0];
 159
 160   if (d_updated){
 161     actual_set_taps(d_new_taps);
 162     d_updated = false;
 163     return 0;                           // output multiple may have changed
 164   }
 165
 166   assert(noutput_items % d_nsamples == 0);
 167
 168   int dec_ctr = 0;
 169   int j = 0;
 170   int ninput_items = noutput_items * decimation();
 171
 172   for (int i = 0; i < ninput_items; i += d_nsamples){
 173
 174     memcpy(d_fwdfft->get_inbuf(), &in[i], d_nsamples * sizeof(float));
 175
 176     for (j = d_nsamples; j < d_fftsize; j++)
 177       d_fwdfft->get_inbuf()[j] = 0;
 178
 179     d_fwdfft->execute();        // compute fwd xform
 180
 181     gr_complex *a = d_fwdfft->get_outbuf();
 182     gr_complex *b = &d_xformed_taps[0];
 183     gr_complex *c = d_invfft->get_inbuf();
 184
 185     for (j = 0; j < d_fftsize/2+1; j++) // filter in the freq domain
 186       c[j] = a[j] * b[j];
 187
 188     d_invfft->execute();        // compute inv xform
 189
 190     // add in the overlapping tail
 191
 192     for (j = 0; j < tailsize(); j++)
 193       d_invfft->get_outbuf()[j] += d_tail[j];
 194
 195     // copy nsamples to output
 196
 197     //memcpy(out, d_invfft->get_outbuf(), d_nsamples * sizeof(float));
 198     //out += d_nsamples;
 199
 200     j = dec_ctr;
 201     while (j < d_nsamples) {
 202       *out++ = d_invfft->get_outbuf()[j];
 203       j += decimation();
 204     }
 205     dec_ctr = (j - d_nsamples);
 206
 207     // stash the tail
 208     memcpy(&d_tail[0], d_invfft->get_outbuf() + d_nsamples,
 209            tailsize() * sizeof(float));
 210   }
 211
 212   assert((out - (float *) output_items[0]) == noutput_items);
 213   assert(dec_ctr == 0);
 214
 215   return noutput_items;
 216 }