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

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2004 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 2, 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_simple_correlator.h>
  28 #include <gr_simple_framer_sync.h>
  29 #include <gr_io_signature.h>
  30 #include <assert.h>
  31 #include <stdexcept>
  32 #include <gr_count_bits.h>
  33
  34
  35 static const int THRESHOLD = 3;
  36
  37 gr_simple_correlator_sptr
  38 gr_make_simple_correlator (int payload_bytesize)
  39 {
  40   return gr_simple_correlator_sptr (new gr_simple_correlator (payload_bytesize));
  41 }
  42
  43 gr_simple_correlator::gr_simple_correlator (int payload_bytesize)
  44   : gr_block ("simple_correlator",
  45               gr_make_io_signature (1, 1, sizeof (float)),
  46               gr_make_io_signature (1, 1, sizeof (unsigned char))),
  47     d_payload_bytesize (payload_bytesize),
  48     d_state (ST_LOOKING), d_osi (0),
  49     d_bblen ((payload_bytesize + GRSF_PAYLOAD_OVERHEAD) * GRSF_BITS_PER_BYTE),
  50     d_bitbuf (new unsigned char [d_bblen]),
  51     d_bbi (0)
  52 {
  53   d_avbi = 0;
  54   d_accum = 0.0;
  55   d_avg = 0.0;
  56   for (int i = 0; i < AVG_PERIOD; i++)
  57     d_avgbuf[i] = 0.0;
  58
  59 #ifdef DEBUG_SIMPLE_CORRELATOR
  60   d_debug_fp = fopen("corr.log", "w");
  61 #endif
  62   enter_looking ();
  63
  64 }
  65
  66 gr_simple_correlator::~gr_simple_correlator ()
  67 {
  68 #ifdef DEBUG_SIMPLE_CORRELATOR
  69   fclose(d_debug_fp);
  70 #endif
  71   delete [] d_bitbuf;
  72 }
  73
  74
  75 void
  76 gr_simple_correlator::enter_looking ()
  77 {
  78   fflush (stdout);
  79   // fprintf (stderr, ">>> enter_looking\n");
  80   d_state = ST_LOOKING;
  81   for (int i = 0; i < OVERSAMPLE; i++)
  82     d_shift_reg[i] = 0;
  83   d_osi = 0;
  84
  85   d_avbi = 0;
  86   d_avg = d_avg * 0.5;
  87   d_accum = 0;
  88   for (int i = 0; i < AVG_PERIOD; i++)
  89     d_avgbuf[i] = 0.0;
  90 }
  91
  92 void
  93 gr_simple_correlator::enter_under_threshold ()
  94 {
  95   fflush (stdout);
  96   // fprintf (stderr, ">>> enter_under_threshold\n");
  97   d_state = ST_UNDER_THRESHOLD;
  98   d_transition_osi = d_osi;
  99 }
 100
 101 void
 102 gr_simple_correlator::enter_locked ()
 103 {
 104   d_state = ST_LOCKED;
 105   int delta = sub_index (d_osi, d_transition_osi);
 106   d_center_osi = add_index (d_transition_osi, delta/2);
 107   d_center_osi = add_index (d_center_osi, 3);   // FIXME
 108   d_bbi = 0;
 109   fflush (stdout);
 110   // fprintf (stderr, ">>> enter_locked  d_center_osi = %d\n", d_center_osi);
 111
 112   d_avg = std::max(-1.0, std::min(1.0, d_accum * (1.0/AVG_PERIOD)));
 113   // fprintf(stderr, ">>> enter_locked  d_avg = %g\n", d_avg);
 114 }
 115
 116 static void
 117 packit (unsigned char *pktbuf, const unsigned char *bitbuf, int bitcount)
 118 {
 119   for (int i = 0; i < bitcount; i += 8){
 120     int t = bitbuf[i+0] & 0x1;
 121     t = (t << 1) | (bitbuf[i+1] & 0x1);
 122     t = (t << 1) | (bitbuf[i+2] & 0x1);
 123     t = (t << 1) | (bitbuf[i+3] & 0x1);
 124     t = (t << 1) | (bitbuf[i+4] & 0x1);
 125     t = (t << 1) | (bitbuf[i+5] & 0x1);
 126     t = (t << 1) | (bitbuf[i+6] & 0x1);
 127     t = (t << 1) | (bitbuf[i+7] & 0x1);
 128     *pktbuf++ = t;
 129   }
 130 }
 131
 132 void
 133 gr_simple_correlator::update_avg(float x)
 134 {
 135   d_accum -= d_avgbuf[d_avbi];
 136   d_avgbuf[d_avbi] = x;
 137   d_accum += x;
 138   d_avbi = (d_avbi + 1) & (AVG_PERIOD-1);
 139 }
 140
 141
 142 int
 143 gr_simple_correlator::general_work (int noutput_items,
 144                                     gr_vector_int &ninput_items,
 145                                     gr_vector_const_void_star &input_items,
 146                                     gr_vector_void_star &output_items)
 147 {
 148   const float *in = (const float *) input_items[0];
 149   unsigned char *out = (unsigned char *) output_items[0];
 150
 151
 152   int n = 0;
 153   int nin = ninput_items[0];
 154   int decision;
 155   int hamming_dist;
 156
 157   struct debug_data {
 158     float       raw_data;
 159     float       sampled;
 160     float       enter_locked;
 161   } debug_data;
 162
 163   while (n < nin){
 164
 165 #ifdef DEBUG_SIMPLE_CORRELATOR
 166     debug_data.raw_data = in[n];
 167     debug_data.sampled = 0.0;
 168     debug_data.enter_locked = 0.0;
 169 #endif
 170
 171     switch (d_state){
 172
 173     case ST_LOCKED:
 174       if (d_osi == d_center_osi){
 175
 176 #ifdef DEBUG_SIMPLE_CORRELATOR
 177         debug_data.sampled = 1.0;
 178 #endif
 179         decision = slice (in[n]);
 180
 181         d_bitbuf[d_bbi] = decision;
 182         d_bbi++;
 183         if (d_bbi >= d_bblen){
 184           // printf ("got whole packet\n");
 185           unsigned char pktbuf[d_bblen/GRSF_BITS_PER_BYTE];
 186           packit (pktbuf, d_bitbuf, d_bbi);
 187           printf ("seqno %3d\n", pktbuf[0]);
 188           memcpy (out, &pktbuf[GRSF_PAYLOAD_OVERHEAD], d_payload_bytesize);
 189           enter_looking ();
 190           consume_each (n + 1);
 191           return d_payload_bytesize;
 192         }
 193       }
 194       break;
 195
 196     case ST_LOOKING:
 197     case ST_UNDER_THRESHOLD:
 198       update_avg(in[n]);
 199       decision = slice (in[n]);
 200       d_shift_reg[d_osi] = (d_shift_reg[d_osi] << 1) | decision;
 201
 202       hamming_dist = gr_count_bits64 (d_shift_reg[d_osi] ^ GRSF_SYNC);
 203       // printf ("%2d  %d\n", hamming_dist, d_osi);
 204
 205       if (d_state == ST_LOOKING && hamming_dist <= THRESHOLD){
 206         // We're seeing a good PN code, remember location
 207         enter_under_threshold ();
 208       }
 209       else if (d_state == ST_UNDER_THRESHOLD && hamming_dist > THRESHOLD){
 210         // no longer seeing good PN code, compute center of goodness
 211         enter_locked ();
 212         debug_data.enter_locked = 1.0;
 213       }
 214       break;
 215
 216     default:
 217       assert (0);
 218     }
 219
 220 #ifdef DEBUG_SIMPLE_CORRELATOR
 221     fwrite(&debug_data, sizeof (debug_data), 1, d_debug_fp);
 222 #endif
 223
 224     d_osi = add_index (d_osi, 1);
 225     n++;
 226   }
 227
 228   consume_each (n);
 229   return 0;
 230 }