git.gag.com Git - debian/gnuradio/blob - gr-noaa/lib/noaa_hrpt_pll_cf.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 <noaa_hrpt_pll_cf.h>
  28 #include <gr_io_signature.h>
  29 #include <gr_math.h>
  30 #include <gr_sincos.h>
  31
  32 #define M_TWOPI (2*M_PI)
  33
  34 noaa_hrpt_pll_cf_sptr
  35 noaa_make_hrpt_pll_cf(float alpha, float beta, float max_offset)
  36 {
  37   return gnuradio::get_initial_sptr(new noaa_hrpt_pll_cf(alpha, beta, max_offset));
  38 }
  39
  40 noaa_hrpt_pll_cf::noaa_hrpt_pll_cf(float alpha, float beta, float max_offset)
  41   : gr_sync_block("noaa_hrpt_pll_cf",
  42                   gr_make_io_signature(1, 1, sizeof(gr_complex)),
  43                   gr_make_io_signature(1, 1, sizeof(float))),
  44     d_alpha(alpha), d_beta(beta), d_max_offset(max_offset),
  45     d_phase(0.0), d_freq(0.0)
  46 {
  47 }
  48
  49 float
  50 phase_wrap(float phase)
  51 {
  52   while (phase < -M_PI)
  53     phase += M_TWOPI;
  54   while (phase > M_PI)
  55     phase -= M_TWOPI;
  56
  57   return phase;
  58 }
  59
  60 int
  61 noaa_hrpt_pll_cf::work(int noutput_items,
  62                        gr_vector_const_void_star &input_items,
  63                        gr_vector_void_star &output_items)
  64 {
  65   const gr_complex *in = (const gr_complex *) input_items[0];
  66   float *out = (float *) output_items[0];
  67
  68   for (int i = 0; i < noutput_items; i++) {
  69
  70     // Adjust PLL phase/frequency
  71     float error = phase_wrap(gr_fast_atan2f(in[i].imag(), in[i].real()) - d_phase);
  72     d_freq  = gr_branchless_clip(d_freq + error*d_beta, d_max_offset);
  73     d_phase = phase_wrap(d_phase + error*d_alpha + d_freq);
  74
  75     // Generate and mix out carrier
  76     float re, im;
  77     gr_sincosf(d_phase, &im, &re);
  78     out[i] = (in[i]*gr_complex(re, -im)).imag();
  79   }
  80
  81   return noutput_items;
  82 }