add tone detect block

git-svn-id: http://op25.osmocom.org/svn/trunk@298 65a5c917-d112-43f1-993d-58c26a4786be

6 files changed, 439 insertions, 0 deletions

diff --git a/repeater/src/lib/Makefile.am b/repeater/src/lib/Makefile.am
index 20d95a5..6a0bc12 100644
--- a/repeater/src/lib/Makefile.am
+++ b/repeater/src/lib/Makefile.am
@@ -68,6 +68,8 @@ _repeater_la_SOURCES = 	repeater\
 	repeater_squelch_base_ff.cc	\
 	repeater_vocoder.cc		\
 	repeater_gardner_costas_cc.cc	\
+	repeater_tdetect_cc.cc		\
+	cic_filter.cc			\
 	repeater_chan_usrp.cc		\
 	repeater_chan_usrp_rx.cc	\
 	p25_framer.cc			\
diff --git a/repeater/src/lib/cic_filter.cc b/repeater/src/lib/cic_filter.cc
new file mode 100644
index 0000000..a4a21e0
--- /dev/null
+++ b/repeater/src/lib/cic_filter.cc
@@ -0,0 +1,103 @@
+/*
+ * routines for CIC filter, delay line
+ *
+ * Copyright 2012, KA1RBI
+ *
+ * This file is part of OP25.
+ *
+ * OP25 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.
+ *
+ * OP25 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 OP25; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+using namespace std;
+
+#include "cic_filter.h"
+
+integrator::integrator(void) {
+//	d_l = 0;
+	d_sum = 0;
+}
+
+integrator::~integrator(void) {
+	// no op
+}
+
+int64_t integrator::cycle(int64_t c1) {
+	d_sum = c1 - d_sum;
+	return (d_sum);
+}
+
+comb::comb(int max_d) {
+	d_max_d = max_d;
+	d_l = new int64_t[d_max_d];
+	for (int i1=0; i1 < d_max_d; i1++) {
+		d_l[i1]=0;
+	}
+	d_cur_d = 0;
+}
+
+comb::~comb(void) {
+	delete [] d_l;
+}
+
+int64_t comb::cycle(int64_t c1, int idx) {
+	assert (idx <= d_max_d);
+	assert (idx >= 0);
+	int i = d_cur_d - idx;
+	if (i < 0)
+		i += d_max_d;
+	int64_t t1 = d_l [ i ];
+	d_l [ d_cur_d ] = c1;
+	d_cur_d += 1;
+	if (d_cur_d >= d_max_d)
+		d_cur_d = 0;
+	return (c1 - t1);
+}
+
+delay::delay(int max_d) {
+	d_max_d = max_d;
+	d_l = new gr_complex[d_max_d];
+	for (int i1=0; i1 < d_max_d; i1++) {
+		d_l[i1]=0;
+	}
+	d_cur_d = 1;
+}
+
+delay::~delay(void) {
+	delete [] d_l;
+}
+
+gr_complex delay::get(int idx) {
+	assert (idx <= d_max_d);
+	assert (idx >= 0);
+	int i = d_cur_d - idx;
+	if (i < 0)
+		i += d_max_d;
+	return d_l [ i ];
+}
+
+gr_complex delay::cycle(gr_complex c1, int idx) {
+	assert (idx <= d_max_d);
+	assert (idx >= 0);
+	int i = d_cur_d - idx;
+	if (i < 0)
+		i += d_max_d;
+	gr_complex t1 = d_l [ i ];
+	d_l [ d_cur_d ] = c1;
+	d_cur_d += 1;
+	if (d_cur_d >= d_max_d)
+		d_cur_d = 0;
+	return t1;
+}
diff --git a/repeater/src/lib/cic_filter.h b/repeater/src/lib/cic_filter.h
new file mode 100644
index 0000000..e5e34a7
--- /dev/null
+++ b/repeater/src/lib/cic_filter.h
@@ -0,0 +1,64 @@
+/*
+ * routines for CIC filter, delay line
+ *
+ * Copyright 2012, KA1RBI
+ *
+ * This file is part of OP25.
+ *
+ * OP25 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.
+ *
+ * OP25 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 OP25; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifndef INCLUDED_CIC_FILTER_H
+#define INCLUDED_CIC_FILTER_H 1
+
+#include <stdint.h>
+#include <assert.h>
+#include <gr_complex.h>
+
+class integrator { /* integrator */
+public:
+	integrator();
+	~integrator();
+	int64_t cycle(int64_t c1);
+private:
+//	int64_t d_l;
+	int64_t d_sum;
+};
+
+class comb { /* comb */
+public:
+	comb(int max_d);
+	~comb();
+	int64_t cycle(int64_t c1, int idx);
+private:
+	int64_t *d_l;
+	int d_max_d;
+	int d_cur_d;
+};
+
+class delay { /* delay */
+public:
+	delay(int max_d);
+	~delay();
+	gr_complex get(int idx);
+	gr_complex cycle(gr_complex c1, int idx);
+private:
+	gr_complex *d_l;
+	int d_max_d;
+	int d_cur_d;
+};
+
+#endif /* INCLUDED_CIC_FILTER_H */
diff --git a/repeater/src/lib/repeater.i b/repeater/src/lib/repeater.i
index 3edb02c..2a539a3 100644
--- a/repeater/src/lib/repeater.i
+++ b/repeater/src/lib/repeater.i
@@ -14,6 +14,8 @@
 #include "repeater_pipe.h"
 #include "repeater_ctcss_squelch_ff.h"
 #include "repeater_gardner_costas_cc.h"
+#include "repeater_tdetect_cc.h"
+#include "cic_filter.h"
 #include "repeater_vocoder.h"
 #include "repeater_chan_usrp.h"
 #include "repeater_chan_usrp_rx.h"
@@ -122,6 +124,18 @@ class repeater_gardner_costas_cc : public gr_sync_block
 
 // ----------------------------------------------------------------
 
+GR_SWIG_BLOCK_MAGIC(repeater,tdetect_cc);
+
+repeater_tdetect_cc_sptr repeater_make_tdetect_cc (int samples_per_symbol, float step_size, int theta, int cic_length);
+
+class repeater_tdetect_cc : public gr_sync_block
+{
+ private:
+  repeater_tdetect_cc (int samples_per_symbol, float step_size, int theta, int cic_length);
+};
+
+// ----------------------------------------------------------------
+
 GR_SWIG_BLOCK_MAGIC(repeater,vocoder);
 
 repeater_vocoder_sptr repeater_make_vocoder (bool encode_flag, bool verbose_flag, int stretch_amt, char* udp_host, int udp_port, bool raw_vectors_flag);
diff --git a/repeater/src/lib/repeater_tdetect_cc.cc b/repeater/src/lib/repeater_tdetect_cc.cc
new file mode 100644
index 0000000..2894906
--- /dev/null
+++ b/repeater/src/lib/repeater_tdetect_cc.cc
@@ -0,0 +1,167 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2005,2006 Free Software Foundation, Inc.
+ *
+ * Tone detect symbol recovery block for GR - Copyright 2012, KA1RBI
+ *
+ * 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 <gr_io_signature.h>
+#include <gr_prefs.h>
+#include <gr_math.h>
+#include <gr_expj.h>
+#include <repeater_tdetect_cc.h>
+#include <gri_mmse_fir_interpolator_cc.h>
+#include <stdexcept>
+#include <cstdio>
+#include <string.h>
+
+#include "cic_filter.h"
+
+#define VERBOSE_TDETECT 0    // Used for debugging symbol timing loop
+
+// Public constructor
+
+repeater_tdetect_cc_sptr 
+repeater_make_tdetect_cc (int samples_per_symbol, float step_size, int theta, int cic_length)
+{
+  return repeater_tdetect_cc_sptr (new repeater_tdetect_cc (samples_per_symbol, step_size, theta, cic_length));
+}
+
+repeater_tdetect_cc::repeater_tdetect_cc (int samples_per_symbol, float step_size, int theta, int cic_length)
+  : gr_block ("repeater_tdetect_cc",
+	      gr_make_io_signature (1, 1, sizeof (gr_complex)),
+	      gr_make_io_signature (1, 1, sizeof (gr_complex))),
+    d_samples_per_symbol(samples_per_symbol),
+    d_half_sps(samples_per_symbol >> 1),
+    d_step_size(step_size),
+    d_theta(theta),
+    d_cic_length(cic_length),
+    d_integrator(), d_comb(cic_length), input_delay(samples_per_symbol),
+    d_l2ctr(0),
+    d_delta(0),
+    d_delta_c(0),
+    d_previous_phase_offset(0)
+{
+  assert((samples_per_symbol & 1) == 0);	// sps must be even
+  set_relative_rate (1.0 / (float) samples_per_symbol);
+  set_history(samples_per_symbol * 2);			// ensure extra input is available
+}
+
+repeater_tdetect_cc::~repeater_tdetect_cc ()
+{
+}
+
+void
+repeater_tdetect_cc::forecast(int noutput_items, gr_vector_int &ninput_items_required)
+{
+  unsigned ninputs = ninput_items_required.size();
+  for (unsigned i=0; i < ninputs; i++)
+    ninput_items_required[i] =
+      (int) ceil((noutput_items * d_samples_per_symbol));
+}
+
+/*
+ * Tone detect symbol recovery block for GR - Copyright 2012, KA1RBI
+ *
+ * symbol timing synchronization using tone detection
+ *
+ * CQPSK signals when AM-demodulated contain a strong tone at 4,800 Hz.
+ * This tone is filtered (using a CIC to remove the DC offset at zero Hz).,
+ * amplified, and decimated.  The resulting error signal is applied to steer
+ * the symbol sampling point toward the optimum phase.
+ *
+ * NOTE: input samples should be normalized (AGC) such that the range of
+ *       signal magnitudes is in the standard zone (0 through +1.0).
+ *
+ *
+ * Source: Software Radios (Second Ed.) B. Farhang-Boroujeny, Sec. 10.2.3
+ *
+ */
+
+int
+repeater_tdetect_cc::general_work (int noutput_items,
+				   gr_vector_int &ninput_items,
+				   gr_vector_const_void_star &input_items,
+				   gr_vector_void_star &output_items)
+{
+  const gr_complex *in = (const gr_complex *) input_items[0];
+  gr_complex *out = (gr_complex *) output_items[0];
+
+  int i=0, o=0;
+  gr_complex sample;
+
+  while((o < noutput_items) && (i < ninput_items[0])) {
+    sample = in[ i++ ];
+    sample = input_delay.cycle(sample, d_delta);
+    if (++d_l2ctr < d_half_sps)
+      continue;	// decimate by sps/2
+    d_l2ctr = 0;
+    int64_t s = (int64_t) (262143.0 * (pow(sample.real(), 2.0) + pow(sample.imag(), 2.0))); /* mag sq */
+    s = d_comb.cycle(s, d_cic_length);
+    s = d_integrator.cycle(s);
+    if (++d_d2ctr & 1)
+      continue;	// decimate by 2
+    float symbol_error = d_step_size * (float)s;
+
+    // now adjust delta_continuous by the amount of the symbol timing error
+    d_delta_c += symbol_error;
+    while (d_delta_c > d_samples_per_symbol)
+      d_delta_c -= d_samples_per_symbol;
+    while (d_delta_c < 0)
+      d_delta_c += d_samples_per_symbol;
+    d_delta = (int) rint(d_delta_c);	// quantize to nearest int
+
+    // d_theta sets optimum sampling point phase offset (delay),
+    // in one-sample units
+    int phase_offset = d_delta + d_theta;
+    while (phase_offset > d_samples_per_symbol)
+      phase_offset -= d_samples_per_symbol;
+    while (phase_offset < 0)
+      phase_offset += d_samples_per_symbol;
+
+    // handle frequency mismatch between local clock and extracted clock
+    // when mismatch reaches a full cycle we must either insert one "extra"
+    // symbol or skip one symbol (depending on algebraic sign of mismatch)
+    int dd = phase_offset - d_previous_phase_offset;
+    int skip_store = 0;
+    if (abs(dd) >= d_half_sps) {
+      if (dd < 0 && o < noutput_items-1) {
+        sample = input_delay.get(d_previous_phase_offset);
+        out[o++] = sample;
+      }
+      if (dd > 0) {
+        skip_store = 1;
+      }
+    }
+    d_previous_phase_offset = phase_offset;
+
+    if (!skip_store) {
+      sample = input_delay.get(phase_offset);
+      out[o++] = sample;
+    }
+  }
+
+  consume_each(i);
+  return o;
+}
diff --git a/repeater/src/lib/repeater_tdetect_cc.h b/repeater/src/lib/repeater_tdetect_cc.h
new file mode 100644
index 0000000..ee2f81e
--- /dev/null
+++ b/repeater/src/lib/repeater_tdetect_cc.h
@@ -0,0 +1,89 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2004 Free Software Foundation, Inc.
+ *
+ * Gardner symbol recovery block for GR - Copyright 2010, KA1RBI
+ *
+ * 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.
+ */
+
+#ifndef INCLUDED_REPEATER_TDETECT_CC_H
+#define	INCLUDED_REPEATER_TDETECT_CC_H
+
+#include <gr_block.h>
+#include <gr_complex.h>
+#include <gr_math.h>
+#include <cic_filter.h>
+
+class gri_mmse_fir_interpolator_cc;
+
+class repeater_tdetect_cc;
+typedef boost::shared_ptr<repeater_tdetect_cc> repeater_tdetect_cc_sptr;
+
+// public constructor
+repeater_tdetect_cc_sptr 
+repeater_make_tdetect_cc (int samples_per_symbol, float step_size, int theta, int cic_length);
+
+/*!
+ * \brief symbol timing loop using tone detection - complex input, complex output.
+ * \ingroup sync_blk
+ *
+ * input samples should be within normalized amplitude range of 0 thru +1.0
+ *
+ * KA1RBI
+ */
+class repeater_tdetect_cc : public gr_block
+{
+ public:
+  ~repeater_tdetect_cc ();
+  void forecast(int noutput_items, gr_vector_int &ninput_items_required);
+  int general_work (int noutput_items,
+		    gr_vector_int &ninput_items,
+		    gr_vector_const_void_star &input_items,
+		    gr_vector_void_star &output_items);
+
+protected:
+  repeater_tdetect_cc (int samples_per_symbol, float step_size, int theta, int cic_length);
+
+private:
+
+  int				d_samples_per_symbol;
+  int				d_half_sps;
+  float				d_step_size;
+  int				d_theta;
+  int				d_cic_length;
+
+  integrator			d_integrator;
+  comb				d_comb;
+  delay				input_delay;
+  int				d_l2ctr;
+  int				d_d2ctr;
+
+  int				d_delta;
+  float				d_delta_c;
+  int				d_previous_phase_offset;
+
+  friend repeater_tdetect_cc_sptr
+  repeater_make_tdetect_cc (int samples_per_symbol, float step_size, int theta, int cic_length);
+
+  float phase_error_detector_qpsk(gr_complex sample);
+  void phase_error_tracking(gr_complex sample);
+
+};
+
+#endif