digital: moved simple_correlator (inverse of simple_framer) to gr-digital.

2 files changed, 232 insertions, 0 deletions

diff --git a/gr-digital/lib/CMakeLists.txt b/gr-digital/lib/CMakeLists.txt
index 6f83ff036..7ac16602c 100644
--- a/gr-digital/lib/CMakeLists.txt
+++ b/gr-digital/lib/CMakeLists.txt
@@ -136,6 +136,7 @@ list(APPEND gr_digital_sources
     digital_probe_mpsk_snr_est_c.cc
     digital_scrambler_bb.cc
     digital_simple_framer.cc
+    digital_simple_correlator.cc
 )
 
 list(APPEND digital_libs
diff --git a/gr-digital/lib/digital_simple_correlator.cc b/gr-digital/lib/digital_simple_correlator.cc
new file mode 100644
index 000000000..37ef2f1e5
--- /dev/null
+++ b/gr-digital/lib/digital_simple_correlator.cc
@@ -0,0 +1,231 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2004,2010,2013 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * GNU Radio is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ *
+ * GNU Radio is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <digital_simple_correlator.h>
+#include <digital_simple_framer_sync.h>
+#include <gr_io_signature.h>
+#include <gr_count_bits.h>
+#include <assert.h>
+#include <stdexcept>
+#include <string.h>
+#include <cstdio>
+
+static const int THRESHOLD = 3;
+
+digital_simple_correlator_sptr
+digital_make_simple_correlator(int payload_bytesize)
+{
+  return gnuradio::get_initial_sptr
+    (new digital_simple_correlator(payload_bytesize));
+}
+
+digital_simple_correlator::digital_simple_correlator(int payload_bytesize)
+  : gr_block("simple_correlator",
+             gr_make_io_signature(1, 1, sizeof(float)),
+             gr_make_io_signature(1, 1, sizeof(unsigned char))),
+    d_payload_bytesize(payload_bytesize),
+    d_state(ST_LOOKING), d_osi(0),
+    d_bblen((payload_bytesize + GRSF_PAYLOAD_OVERHEAD) * GRSF_BITS_PER_BYTE),
+    d_bitbuf(new unsigned char[d_bblen]),
+    d_pktbuf(new unsigned char[d_bblen/GRSF_BITS_PER_BYTE]),
+    d_bbi(0)
+{
+  d_avbi = 0;
+  d_accum = 0.0;
+  d_avg = 0.0;
+  for(int i = 0; i < AVG_PERIOD; i++)
+    d_avgbuf[i] = 0.0;
+
+#ifdef DEBUG_SIMPLE_CORRELATOR
+  d_debug_fp = fopen("corr.log", "w");
+#endif
+  enter_looking();
+}
+
+digital_simple_correlator::~digital_simple_correlator()
+{
+#ifdef DEBUG_SIMPLE_CORRELATOR
+  fclose(d_debug_fp);
+#endif
+  delete [] d_bitbuf;
+  delete [] d_pktbuf;
+}
+
+void
+digital_simple_correlator::enter_looking()
+{
+  fflush(stdout);
+  // fprintf(stderr, ">>> enter_looking\n");
+  d_state = ST_LOOKING;
+  for(int i = 0; i < OVERSAMPLE; i++)
+    d_shift_reg[i] = 0;
+  d_osi = 0;
+
+  d_avbi = 0;
+  d_avg = d_avg * 0.5;
+  d_accum = 0;
+  for(int i = 0; i < AVG_PERIOD; i++)
+    d_avgbuf[i] = 0.0;
+}
+
+void
+digital_simple_correlator::enter_under_threshold()
+{
+  fflush(stdout);
+  // fprintf(stderr, ">>> enter_under_threshold\n");
+  d_state = ST_UNDER_THRESHOLD;
+  d_transition_osi = d_osi;
+}
+
+void
+digital_simple_correlator::enter_locked()
+{
+  d_state = ST_LOCKED;
+  int delta = sub_index(d_osi, d_transition_osi);
+  d_center_osi = add_index(d_transition_osi, delta/2);
+  //d_center_osi = add_index(d_center_osi, 3);   // FIXME
+  d_bbi = 0;
+  fflush(stdout);
+  // fprintf(stderr, ">>> enter_locked  d_center_osi = %d\n", d_center_osi);
+  
+  d_avg = std::max(-1.0, std::min(1.0, d_accum * (1.0/AVG_PERIOD)));
+  // fprintf(stderr, ">>> enter_locked  d_avg = %g\n", d_avg);
+}
+
+static void
+packit(unsigned char *pktbuf, const unsigned char *bitbuf, int bitcount)
+{
+  for(int i = 0; i < bitcount; i += 8) {
+    int t = bitbuf[i+0] & 0x1;
+    t = (t << 1) | (bitbuf[i+1] & 0x1);
+    t = (t << 1) | (bitbuf[i+2] & 0x1);
+    t = (t << 1) | (bitbuf[i+3] & 0x1);
+    t = (t << 1) | (bitbuf[i+4] & 0x1);
+    t = (t << 1) | (bitbuf[i+5] & 0x1);
+    t = (t << 1) | (bitbuf[i+6] & 0x1);
+    t = (t << 1) | (bitbuf[i+7] & 0x1);
+    *pktbuf++ = t;
+  }
+}
+
+void
+digital_simple_correlator::update_avg(float x)
+{
+  d_accum -= d_avgbuf[d_avbi];
+  d_avgbuf[d_avbi] = x;
+  d_accum += x;
+  d_avbi = (d_avbi + 1) & (AVG_PERIOD-1);
+}
+
+int
+digital_simple_correlator::general_work(int noutput_items,
+                                        gr_vector_int &ninput_items,
+                                        gr_vector_const_void_star &input_items,
+                                        gr_vector_void_star &output_items)
+{
+  const float *in = (const float*)input_items[0];
+  unsigned char *out = (unsigned char*)output_items[0];
+
+  int n = 0;
+  int nin = ninput_items[0];
+  int decision;
+  int hamming_dist;
+    
+#ifdef DEBUG_SIMPLE_CORRELATOR
+  struct debug_data {
+    float raw_data;
+    float sampled;
+    float enter_locked;
+  } debug_data;
+#endif
+
+  while(n < nin) {
+
+#ifdef DEBUG_SIMPLE_CORRELATOR
+    debug_data.raw_data = in[n];
+    debug_data.sampled = 0.0;
+    debug_data.enter_locked = 0.0;
+#endif
+
+    switch(d_state) {
+    case ST_LOCKED:
+      if(d_osi == d_center_osi) {
+
+#ifdef DEBUG_SIMPLE_CORRELATOR
+        debug_data.sampled = 1.0;
+#endif
+        decision = slice(in[n]);
+
+        d_bitbuf[d_bbi] = decision;
+        d_bbi++;
+        if(d_bbi >= d_bblen) {
+          // printf("got whole packet\n");
+          packit(d_pktbuf, d_bitbuf, d_bbi);
+          //printf("seqno %3d\n", d_pktbuf[0]);
+          memcpy(out, &d_pktbuf[GRSF_PAYLOAD_OVERHEAD], d_payload_bytesize);
+          enter_looking();
+          consume_each(n + 1);
+          return d_payload_bytesize;
+        }
+      }
+      break;
+
+    case ST_LOOKING:
+    case ST_UNDER_THRESHOLD:
+      update_avg(in[n]);
+      decision = slice(in[n]);
+
+      d_shift_reg[d_osi] = (d_shift_reg[d_osi] << 1) | decision;
+      hamming_dist = gr_count_bits64(d_shift_reg[d_osi] ^ GRSF_SYNC);
+      //fprintf(stderr, "%2d  %d\n", hamming_dist, d_osi);
+
+      if(d_state == ST_LOOKING && hamming_dist <= THRESHOLD) {
+        // We're seeing a good PN code, remember location
+        enter_under_threshold();
+      }
+      else if(d_state == ST_UNDER_THRESHOLD && hamming_dist > THRESHOLD) {
+        // no longer seeing good PN code, compute center of goodness
+        enter_locked();
+#ifdef DEBUG_SIMPLE_CORRELATOR
+        debug_data.enter_locked = 1.0;
+#endif
+      }
+      break;
+    default:
+      assert(0);
+    }
+
+#ifdef DEBUG_SIMPLE_CORRELATOR
+    fwrite(&debug_data, sizeof(debug_data), 1, d_debug_fp);
+#endif
+
+    d_osi = add_index(d_osi, 1);
+    n++;
+  }
+
+  consume_each(n);
+  return 0;
+}