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