git.gag.com Git - debian/gnuradio/blob - gnuradio-core/src/lib/general/gr_costas_loop_cc.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2006 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_costas_loop_cc.h>
  28 #include <gr_io_signature.h>
  29 #include <gr_expj.h>
  30 #include <gr_sincos.h>
  31 #include <math.h>
  32
  33 #define M_TWOPI (2*M_PI)
  34
  35 gr_costas_loop_cc_sptr
  36 gr_make_costas_loop_cc (float alpha, float beta,
  37                         float max_freq, float min_freq,
  38                         int order
  39                         ) throw (std::invalid_argument)
  40 {
  41   return gr_costas_loop_cc_sptr (new gr_costas_loop_cc (alpha, beta,
  42                                                         max_freq, min_freq,
  43                                                         order));
  44 }
  45
  46 gr_costas_loop_cc::gr_costas_loop_cc (float alpha, float beta,
  47                                       float max_freq, float min_freq,
  48                                       int order
  49                                       ) throw (std::invalid_argument)
  50   : gr_sync_block ("costas_loop_cc",
  51                    gr_make_io_signature (1, 1, sizeof (gr_complex)),
  52                    gr_make_io_signature (1, 2, sizeof (gr_complex))),
  53     d_alpha(alpha), d_beta(beta),
  54     d_max_freq(max_freq), d_min_freq(min_freq),
  55     d_phase(0), d_freq((max_freq+min_freq)/2),
  56     d_order(order), d_phase_detector(0)
  57 {
  58   switch(d_order) {
  59   case 2:
  60     d_phase_detector = &gr_costas_loop_cc::phase_detector_2;
  61     break;
  62
  63   case 4:
  64     d_phase_detector = &gr_costas_loop_cc::phase_detector_4;
  65     break;
  66
  67   default:
  68     throw std::invalid_argument("order must be 2 or 4");
  69     break;
  70   }
  71 }
  72
  73
  74 float
  75 gr_costas_loop_cc::phase_detector_4(gr_complex sample) const
  76 {
  77
  78   return ((sample.real()>0 ? 1.0 : -1.0) * sample.imag() -
  79           (sample.imag()>0 ? 1.0 : -1.0) * sample.real());
  80 }
  81
  82 float
  83 gr_costas_loop_cc::phase_detector_2(gr_complex sample) const
  84 {
  85   return (sample.real()*sample.imag());
  86 }
  87
  88 int
  89 gr_costas_loop_cc::work (int noutput_items,
  90                          gr_vector_const_void_star &input_items,
  91                          gr_vector_void_star &output_items)
  92 {
  93   const gr_complex *iptr = (gr_complex *) input_items[0];
  94   gr_complex *optr = (gr_complex *) output_items[0];
  95   gr_complex *foptr = (gr_complex *) output_items[1];
  96
  97   bool write_foptr = output_items.size() >= 2;
  98
  99   float error;
 100   gr_complex nco_out;
 101
 102   if (write_foptr) {
 103
 104     for (int i = 0; i < noutput_items; i++){
 105       nco_out = gr_expj(-d_phase);
 106       optr[i] = iptr[i] * nco_out;
 107
 108       error = (*this.*d_phase_detector)(optr[i]);
 109       if (error > 1)
 110         error = 1;
 111       else if (error < -1)
 112         error = -1;
 113
 114       d_freq = d_freq + d_beta * error;
 115       d_phase = d_phase + d_freq + d_alpha * error;
 116
 117       while(d_phase>M_TWOPI)
 118         d_phase -= M_TWOPI;
 119       while(d_phase<-M_TWOPI)
 120         d_phase += M_TWOPI;
 121
 122       if (d_freq > d_max_freq)
 123         d_freq = d_max_freq;
 124       else if (d_freq < d_min_freq)
 125         d_freq = d_min_freq;
 126
 127       foptr[i] = gr_complex(d_freq,0);
 128     }
 129   } else {
 130     for (int i = 0; i < noutput_items; i++){
 131       nco_out = gr_expj(-d_phase);
 132       optr[i] = iptr[i] * nco_out;
 133
 134       error = (*this.*d_phase_detector)(optr[i]);
 135       if (error > 1)
 136         error = 1;
 137       else if (error < -1)
 138         error = -1;
 139
 140       d_freq = d_freq + d_beta * error;
 141       d_phase = d_phase + d_freq + d_alpha * error;
 142
 143       while(d_phase>M_TWOPI)
 144         d_phase -= M_TWOPI;
 145       while(d_phase<-M_TWOPI)
 146         d_phase += M_TWOPI;
 147
 148       if (d_freq > d_max_freq)
 149         d_freq = d_max_freq;
 150       else if (d_freq < d_min_freq)
 151         d_freq = d_min_freq;
 152
 153     }
 154   }
 155   return noutput_items;
 156 }