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

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2009 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 <cstdio>
  28 #include <cmath>
  29
  30 #include <gr_pfb_clock_sync_ccf.h>
  31 #include <gr_fir_ccf.h>
  32 #include <gr_fir_util.h>
  33 #include <gr_io_signature.h>
  34 #include <gr_math.h>
  35
  36 gr_pfb_clock_sync_ccf_sptr gr_make_pfb_clock_sync_ccf (float sps, float gain,
  37                                                        const std::vector<float> &taps,
  38                                                        unsigned int filter_size,
  39                                                        float init_phase)
  40 {
  41   return gr_pfb_clock_sync_ccf_sptr (new gr_pfb_clock_sync_ccf (sps, gain, taps,
  42                                                                 filter_size,
  43                                                                 init_phase));
  44 }
  45
  46
  47 gr_pfb_clock_sync_ccf::gr_pfb_clock_sync_ccf (float sps, float gain,
  48                                               const std::vector<float> &taps,
  49                                               unsigned int filter_size,
  50                                               float init_phase)
  51   : gr_block ("pfb_clock_sync_ccf",
  52               gr_make_io_signature (1, 1, sizeof(gr_complex)),
  53               gr_make_io_signature2 (2, 2, sizeof(gr_complex), sizeof(float))),
  54     d_updated (false), d_sps(sps), d_alpha(gain)
  55 {
  56   d_nfilters = filter_size;
  57
  58   // Store the last filter between calls to work
  59   // The accumulator keeps track of overflow to increment the stride correctly.
  60   // set it here to the fractional difference based on the initial phaes
  61   // assert(init_phase <= 2*M_PI);
  62   float x = init_phase / (2*M_PI); //normalize initial phase
  63   d_acc = x*(d_nfilters-1);
  64   d_last_filter = (int)floor(d_acc);
  65   d_acc = fmodf(d_acc, 1);
  66   d_start_count = 0;
  67
  68
  69   d_filters = std::vector<gr_fir_ccf*>(d_nfilters);
  70   d_diff_filters = std::vector<gr_fir_ccf*>(d_nfilters);
  71
  72   // Create an FIR filter for each channel and zero out the taps
  73   std::vector<float> vtaps(0, d_nfilters);
  74   for(unsigned int i = 0; i < d_nfilters; i++) {
  75     d_filters[i] = gr_fir_util::create_gr_fir_ccf(vtaps);
  76     d_diff_filters[i] = gr_fir_util::create_gr_fir_ccf(vtaps);
  77   }
  78
  79   // Now, actually set the filters' taps
  80   std::vector<float> dtaps;
  81   create_diff_taps(taps, dtaps);
  82   set_taps(taps, d_taps, d_filters);
  83   set_taps(dtaps, d_dtaps, d_diff_filters);
  84 }
  85
  86 gr_pfb_clock_sync_ccf::~gr_pfb_clock_sync_ccf ()
  87 {
  88   for(unsigned int i = 0; i < d_nfilters; i++) {
  89     delete d_filters[i];
  90   }
  91 }
  92
  93 void
  94 gr_pfb_clock_sync_ccf::set_taps (const std::vector<float> &newtaps,
  95                                  std::vector< std::vector<float> > &ourtaps,
  96                                  std::vector<gr_fir_ccf*> &ourfilter)
  97 {
  98   unsigned int i,j;
  99
 100   unsigned int ntaps = newtaps.size();
 101   d_taps_per_filter = (unsigned int)ceil((double)ntaps/(double)d_nfilters);
 102
 103   // Create d_numchan vectors to store each channel's taps
 104   ourtaps.resize(d_nfilters);
 105
 106   // Make a vector of the taps plus fill it out with 0's to fill
 107   // each polyphase filter with exactly d_taps_per_filter
 108   std::vector<float> tmp_taps;
 109   tmp_taps = newtaps;
 110   while((float)(tmp_taps.size()) < d_nfilters*d_taps_per_filter) {
 111     tmp_taps.push_back(0.0);
 112   }
 113
 114   // Partition the filter
 115   for(i = 0; i < d_nfilters; i++) {
 116     // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
 117     ourtaps[i] = std::vector<float>(d_taps_per_filter, 0);
 118     for(j = 0; j < d_taps_per_filter; j++) {
 119       ourtaps[i][j] = tmp_taps[i + j*d_nfilters];  // add taps to channels in reverse order
 120     }
 121
 122     // Build a filter for each channel and add it's taps to it
 123     ourfilter[i]->set_taps(ourtaps[i]);
 124   }
 125
 126   // Set the history to ensure enough input items for each filter
 127   set_history (d_taps_per_filter + d_sps);
 128
 129   d_updated = true;
 130 }
 131
 132 void
 133 gr_pfb_clock_sync_ccf::create_diff_taps(const std::vector<float> &newtaps,
 134                                         std::vector<float> &difftaps)
 135 {
 136   difftaps.clear();
 137   difftaps.push_back(0); //newtaps[0]);
 138   for(unsigned int i = 1; i < newtaps.size()-1; i++) {
 139     difftaps.push_back(newtaps[i+1] - newtaps[i-1]);
 140   }
 141   difftaps.push_back(0);//-newtaps[newtaps.size()-1]);
 142 }
 143
 144 void
 145 gr_pfb_clock_sync_ccf::print_taps()
 146 {
 147   unsigned int i, j;
 148   for(i = 0; i < d_nfilters; i++) {
 149     printf("filter[%d]: [%.4e, ", i, d_taps[i][0]);
 150     for(j = 1; j < d_taps_per_filter-1; j++) {
 151       printf("%.4e,", d_taps[i][j]);
 152     }
 153     printf("%.4e]\n", d_taps[i][j]);
 154   }
 155 }
 156
 157 void
 158 gr_pfb_clock_sync_ccf::print_diff_taps()
 159 {
 160   unsigned int i, j;
 161   for(i = 0; i < d_nfilters; i++) {
 162     printf("filter[%d]: [%.4e, ", i, d_dtaps[i][0]);
 163     for(j = 1; j < d_taps_per_filter-1; j++) {
 164       printf("%.4e,", d_dtaps[i][j]);
 165     }
 166     printf("%.4e]\n", d_dtaps[i][j]);
 167   }
 168 }
 169
 170
 171 std::vector<float>
 172 gr_pfb_clock_sync_ccf::channel_taps(int channel)
 173 {
 174   std::vector<float> taps;
 175   unsigned int i;
 176   for(i = 0; i < d_taps_per_filter; i++) {
 177     taps.push_back(d_taps[channel][i]);
 178   }
 179   return taps;
 180 }
 181
 182 std::vector<float>
 183 gr_pfb_clock_sync_ccf::diff_channel_taps(int channel)
 184 {
 185   std::vector<float> taps;
 186   unsigned int i;
 187   for(i = 0; i < d_taps_per_filter; i++) {
 188     taps.push_back(d_dtaps[channel][i]);
 189   }
 190   return taps;
 191 }
 192
 193
 194 int
 195 gr_pfb_clock_sync_ccf::general_work (int noutput_items,
 196                                      gr_vector_int &ninput_items,
 197                                      gr_vector_const_void_star &input_items,
 198                                      gr_vector_void_star &output_items)
 199 {
 200   gr_complex *in = (gr_complex *) input_items[0];
 201   gr_complex *out = (gr_complex *) output_items[0];
 202   float *err = (float *) output_items[1];
 203
 204   if (d_updated) {
 205     d_updated = false;
 206     return 0;                // history requirements may have changed.
 207   }
 208
 209   // We need this many to process one output
 210   int nrequired = ninput_items[0] - d_taps_per_filter;
 211
 212   int i = 0, count = d_start_count;
 213   float error = 0;
 214
 215   // produce output as long as we can and there are enough input samples
 216   while((i < noutput_items) && (count < nrequired)) {
 217     out[i] = d_filters[d_last_filter]->filter(&in[count]);
 218     error =  (out[i] * d_diff_filters[d_last_filter]->filter(&in[count])).real();
 219     err[i] = error;
 220
 221     d_acc += d_alpha*error;
 222     gr_branchless_clip(d_acc, 1);
 223
 224     int newfilter;
 225     newfilter = (int)((float)d_last_filter + d_acc);
 226     if(newfilter != (int)d_last_filter)
 227       d_acc = 0.5;
 228
 229     if(newfilter >= (int)d_nfilters) {
 230       d_last_filter = newfilter - d_nfilters;
 231       count++;
 232     }
 233     else if(newfilter < 0) {
 234       d_last_filter = d_nfilters + newfilter;
 235       count--;
 236     }
 237     else {
 238       d_last_filter = newfilter;
 239     }
 240
 241     i++;
 242     count += d_sps;
 243   }
 244
 245   // Set the start index at the next entrance to the work function
 246   // if we stop because we run out of input items, jump ahead in the
 247   // next call to work. Otherwise, we can start at zero.
 248   if(count > nrequired) {
 249     d_start_count = count - (nrequired);
 250     consume_each(ninput_items[0]-d_taps_per_filter);
 251   }
 252   else {
 253     d_start_count = 0;
 254     consume_each(count);
 255   }
 256
 257   return i;
 258 }