gr_simple_correlator.cc

gnuradio软件无线电源程序.现在的手机多基于软件无线电

/* -*- c++ -*- */
/*
 * Copyright 2004 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 2, 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., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

#ifndef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gr_simple_correlator.h>
#include <gr_simple_framer_sync.h>
#include <gr_io_signature.h>
#include <assert.h>
#include <stdexcept>
#include <gr_count_bits.h>


static const int BITS_PER_BYTE = 8;
static const int SYNC_OVERHEAD = 4;
static const int PAYLOAD_OVERHEAD = 2;			// 1 byte seqno, 1 byte cmd
static const int OVERHEAD = SYNC_OVERHEAD + PAYLOAD_OVERHEAD; 
static const int THRESHOLD = 3;

gr_simple_correlator_sptr
gr_make_simple_correlator (int payload_bytesize)
{
  return gr_simple_correlator_sptr (new gr_simple_correlator (payload_bytesize));
}

gr_simple_correlator::gr_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 + PAYLOAD_OVERHEAD) * BITS_PER_BYTE),
    d_bitbuf (new unsigned char [d_bblen]),
    d_bbi (0)
{
  enter_looking ();
}

gr_simple_correlator::~gr_simple_correlator ()
{
  delete [] d_bitbuf;
}
   

void
gr_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;
}

void
gr_simple_correlator::enter_under_threshold ()
{
  fflush (stdout);
  // fprintf (stderr, ">>> enter_under_threshold\n");
  d_state = ST_UNDER_THRESHOLD;
  d_transition_osi = d_osi;
}

void
gr_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_bbi = 0;
  fflush (stdout);
  // fprintf (stderr, ">>> enter_locked  d_center_osi = %d\n", d_center_osi);
}

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;
  }
}

inline static int slice (float x)
{
  return x >= 0 ? 1 : 0;
}

int
gr_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;

  while (n < nin){

    switch (d_state){

    case ST_LOCKED:
      if (d_osi == d_center_osi){
	decision = slice (in[n]);
	
	d_bitbuf[d_bbi] = decision;
	d_bbi++;
	if (d_bbi >= d_bblen){
	  // printf ("got whole packet\n");
	  unsigned char pktbuf[d_bblen/BITS_PER_BYTE];
	  packit (pktbuf, d_bitbuf, d_bbi);
	  printf ("seqno %3d\n", pktbuf[0]);
	  memcpy (out, &pktbuf[2], d_payload_bytesize);
	  enter_looking ();
	  consume_each (n + 1);
	  return d_payload_bytesize;
	}
      }
      break;

    case ST_LOOKING:
    case ST_UNDER_THRESHOLD:
      decision = slice (in[n]);
      d_shift_reg[d_osi] = (d_shift_reg[d_osi] << 1) | decision;

      hamming_dist = gr_count_bits32 (d_shift_reg[d_osi] ^ GRSF_SYNC);
      // printf ("%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 ();
      }
      break;

    default:
      assert (0);
    }
      
    d_osi = add_index (d_osi, 1);
    n++;
  }

  consume_each (n);
  return 0;
}