add correlator block

grc/CMakeLists.txt

@@ -7,5 +7,5 @@
 #
 
 install(FILES
-    DESTINATION share/gnuradio/grc/blocks
+    elen90089_corr_est_cc.block.yml DESTINATION share/gnuradio/grc/blocks
 )

grc/elen90089_corr_est_cc.block.yml

+id: elen90089_corr_est_cc
+label: CDC TS Correlator
+category: '[elen90089]'
+
+templates:
+  imports: import elen90089
+  make: elen90089.corr_est_cc(${sequence}, ${threshold})
+
+parameters:
+- id: sequence
+  label: Sequence
+  dtype: complex_vector
+- id: threshold
+  label: Threshold
+  default: '0.5'
+  dtype: float
+
+inputs:
+- label: in
+  domain: stream
+  dtype: complex
+outputs:
+- label: out
+  domain: stream
+  dtype: complex
+- label: out
+  domain: stream
+  dtype: float
+  optional: True
+
+file_format: 1

include/elen90089/CMakeLists.txt

@@ -11,5 +11,6 @@
 ########################################################################
 install(FILES
     api.h
+    corr_est_cc.h
     DESTINATION include/elen90089
 )

include/elen90089/corr_est_cc.h

+/* -*- c++ -*- */
+/*
+ * Copyright 2022 University of Melbourne.
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#ifndef INCLUDED_ELEN90089_CORR_EST_CC_H
+#define INCLUDED_ELEN90089_CORR_EST_CC_H
+
+#include <elen90089/api.h>
+#include <gnuradio/sync_block.h>
+
+namespace gr {
+namespace elen90089 {
+
+/*!
+ * \brief Search for symbol sequence within sample stream via correlation
+ * \ingroup elen90089
+ *
+ * \details
+ * Input:
+ * \li Stream of complex samples.
+ *
+ * Output:
+ * \li Output stream of input complex samples delayed by sequence length and
+ *     tagged with correlation start and frequency offset estimate
+ * \li tag 'corr_start': start sample of identified sequence
+ * \li tag 'freq_est': estimated frequency offset
+ *
+ * \li Optional 2nd output stream providing advanced correlator output
+ */
+class ELEN90089_API corr_est_cc : virtual public gr::sync_block
+{
+public:
+    typedef std::shared_ptr<corr_est_cc> sptr;
+
+    /*!
+     * Make a block that searchers for the \p sequence vector within input
+     * sample stream and outputs stream tagged with correlation and frequency
+     * offset estimate.
+     *
+     * \param sequence      Symbol sequence to correlate agains.
+     * \param threshold     Correlation threshold to declare sequence found
+     *                      (0.0 to 1.0)
+     */
+    static sptr make(const std::vector<gr_complex>& sequence,
+                     float threshold=0.5);
+
+    virtual std::vector<gr_complex> sequence() const = 0;
+    virtual void set_sequence(const std::vector<gr_complex>& sequence) = 0;
+
+    virtual float threshold() const = 0;
+    virtual void set_threshold(float threshold) = 0;
+};
+
+} // namespace elen90089
+} // namespace gr
+
+#endif /* INCLUDED_ELEN90089_CORR_EST_CC_H */

lib/CMakeLists.txt

@@ -12,6 +12,7 @@
 include(GrPlatform) #define LIB_SUFFIX
 
 list(APPEND elen90089_sources
+    corr_est_cc_impl.cc
 )
 
 set(elen90089_sources "${elen90089_sources}" PARENT_SCOPE)
@@ -21,7 +22,12 @@ if(NOT elen90089_sources)
 endif(NOT elen90089_sources)
 
 add_library(gnuradio-elen90089 SHARED ${elen90089_sources})
-target_link_libraries(gnuradio-elen90089 gnuradio::gnuradio-runtime)
+
+target_link_libraries(
+    gnuradio-elen90089
+    gnuradio::gnuradio-runtime
+    gnuradio-filter)
+
 target_include_directories(gnuradio-elen90089
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include>
     PUBLIC $<INSTALL_INTERFACE:include>

lib/corr_est_cc_impl.cc

+/* -*- c++ -*- */
+/*
+ * Copyright 2022 University of Melbourne.
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#include <gnuradio/io_signature.h>
+#include <gnuradio/math.h>
+#include <volk/volk.h>
+#include "corr_est_cc_impl.h"
+
+namespace gr {
+namespace elen90089 {
+
+corr_est_cc::sptr corr_est_cc::make(const std::vector<gr_complex>& sequence,
+                                    float threshold)
+{
+    return gnuradio::make_block_sptr<corr_est_cc_impl>(sequence, threshold);
+}
+
+corr_est_cc_impl::corr_est_cc_impl(const std::vector<gr_complex>& sequence,
+                                   float threshold)
+    : gr::sync_block("corr_est_cc",
+                     gr::io_signature::make(1, 1, sizeof(gr_complex)),
+                     gr::io_signature::make2(1, 2, sizeof(gr_complex), sizeof(float))),
+      d_src_id(pmt::intern(alias())),
+      d_sequence(sequence.size()),
+      d_threshold(threshold),
+      d_filter(1, sequence),
+      d_corr(s_nitems),
+      d_corr_mag(s_nitems),
+      d_y_mag(s_nitems + sequence.size()),
+      d_y_accum(0),
+      d_skip(0)
+{
+    set_max_noutput_items(s_nitems);
+    set_sequence(sequence);
+}
+
+corr_est_cc_impl::~corr_est_cc_impl() {}
+
+std::vector<gr_complex> corr_est_cc_impl::sequence() const
+{
+    return d_sequence;
+}
+
+void corr_est_cc_impl::set_sequence(const std::vector<gr_complex>& sequence)
+{
+    gr::thread::scoped_lock lock(d_setlock);
+
+    d_sequence = sequence;
+
+    // create time-reversed conjugate of training sequence for filtering
+    std::vector<gr_complex> taps(sequence.size());
+    std::reverse_copy(sequence.begin(), sequence.end(), taps.begin()); 
+    float scale = 0.0;
+    for (size_t i = 0; i < taps.size(); i++) {
+        taps[i] = std::conj(taps[i]);
+        scale += std::norm(taps[i]);
+    }
+    scale = 1.0 / sqrt(scale);
+    for (size_t i = 0; i < taps.size(); i++) {
+        taps[i] *= scale;
+    }
+
+    // set taps and block output multiple to FFT kernel's internal nsamples
+    const int nsamples = d_filter.set_taps(taps);
+    set_output_multiple(nsamples);
+
+    // keep a history of the length of the sync word to delay for tagging
+    set_history(d_sequence.size());
+    declare_sample_delay(0, sequence.size());
+    declare_sample_delay(1, 0);
+}
+
+float corr_est_cc_impl::threshold() const
+{
+    return d_threshold;
+}
+
+void corr_est_cc_impl::set_threshold(float threshold)
+{
+    gr::thread::scoped_lock lock(d_setlock);
+    d_threshold = threshold;
+}
+
+double corr_est_cc_impl::estimate_freq_offset(const gr_complex* samples)
+{
+    double accum = 0;
+
+    gr_complex y_p = samples[0] / d_sequence[0];
+    for (int i = 1; i < d_sequence.size(); i++) {
+        gr_complex y = samples[i] / d_sequence[i];
+        gr_complex p = y * conj(y_p);
+        accum += fast_atan2f(p.imag(), p.real());
+        y_p = y;
+    }
+
+    return (accum / (d_sequence.size() - 1));
+}
+
+int corr_est_cc_impl::work(int noutput_items,
+                           gr_vector_const_void_star &input_items,
+                           gr_vector_void_star &output_items)
+{
+    auto in = static_cast<const gr_complex*>(input_items[0]);
+    auto out = static_cast<gr_complex*>(output_items[0]);
+
+    float* corr_mag;
+    if (output_items.size() > 1) {
+        corr_mag = static_cast<float*>(output_items[1]);
+    } else {
+        corr_mag = d_corr_mag.data();
+    }
+
+    unsigned int hist_len = history() - 1;
+
+    float threshold;
+    {
+        gr::thread::scoped_lock lock(d_setlock);
+        threshold = d_threshold;
+    }
+
+    // correlate input samples with training sequence
+    d_filter.filter(noutput_items, &in[hist_len], d_corr.data());
+
+    // calculate squared magnitude of correlation result
+    volk_32fc_magnitude_squared_32f(corr_mag, d_corr.data(), noutput_items);
+
+    // calculate squared magnitued of input samples for normalization
+    volk_32fc_magnitude_squared_32f(d_y_mag.data(), in, noutput_items + history());
+
+    // search for correlation peaks above threshold
+    for (int i = 0; i < noutput_items; i++) {
+        // add sample to moving average
+        d_y_accum += d_y_mag[hist_len + i];
+
+        // normalize cross-correlation
+        corr_mag[i] /= d_y_accum;
+
+        // drop oldest sample from moving average
+        d_y_accum -= d_y_mag[i]; 
+
+        if (d_skip > 0) {
+            d_skip--;
+            continue;
+        }
+
+        if (corr_mag[i] > d_threshold) {
+            // coarse frequency estimate
+            double freq_est = estimate_freq_offset(&in[i]);
+
+            // tag output
+            add_item_tag(0, nitems_written(0) + i,
+                         pmt::intern("corr_start"),
+                         pmt::from_double(corr_mag[i]),
+                         d_src_id);
+            add_item_tag(0, nitems_written(0) + i,
+                         pmt::intern("freq_est"),
+                         pmt::from_double(freq_est),
+                         d_src_id);
+            if (output_items.size() > 1) {
+                add_item_tag(1, nitems_written(0) + i,
+                             pmt::intern("corr_start"),
+                             pmt::from_double(corr_mag[i]),
+                             d_src_id);
+                add_item_tag(1, nitems_written(0) + i,
+                             pmt::intern("freq_est"),
+                             pmt::from_double(freq_est),
+                             d_src_id);
+            }
+
+            // skip remaining symbols in sequence
+            d_skip = d_sequence.size() - 1;
+        }
+    }
+
+    // pass through input samples but with delay of history()
+    memcpy(out, &in[0], sizeof(gr_complex)*noutput_items);
+
+    return noutput_items;
+}
+
+} /* namespace elen90089 */
+} /* namespace gr */

lib/corr_est_cc_impl.h

+/* -*- c++ -*- */
+/*
+ * Copyright 2022 University of Melbourne.
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#ifndef INCLUDED_ELEN90089_CORR_EST_CC_IMPL_H
+#define INCLUDED_ELEN90089_CORR_EST_CC_IMPL_H
+
+#include <elen90089/corr_est_cc.h>
+#include <gnuradio/filter/fft_filter.h>
+
+using namespace gr::filter;
+
+namespace gr {
+namespace elen90089 {
+
+class corr_est_cc_impl : public corr_est_cc
+{
+private:
+    pmt::pmt_t d_src_id;
+
+    std::vector<gr_complex> d_sequence;
+    float d_threshold;
+
+    kernel::fft_filter_ccc d_filter;
+    volk::vector<gr_complex> d_corr;
+    volk::vector<float> d_corr_mag;
+    volk::vector<float> d_y_mag;
+    float d_y_accum;
+    int d_skip;
+
+    static constexpr int s_nitems = 24*1024;
+
+    double estimate_freq_offset(const gr_complex* samples);
+
+public:
+    corr_est_cc_impl(const std::vector<gr_complex>& sequence,
+                     float threshold = 0.5);
+  
+    ~corr_est_cc_impl() override;
+
+    std::vector<gr_complex> sequence() const override;
+    void set_sequence(const std::vector<gr_complex>& sequence) override;
+
+    float threshold() const override;
+    void set_threshold(float threshold) override;
+
+    int work(int noutput_items,
+             gr_vector_const_void_star &input_items,
+             gr_vector_void_star &output_items) override;
+};
+
+} // namespace elen90089
+} // namespace gr
+
+#endif /* INCLUDED_ELEN90089_CORR_EST_CC_IMPL_H */

python/bindings/CMakeLists.txt

@@ -29,7 +29,7 @@ include(GrPybind)
 ########################################################################
 
 list(APPEND elen90089_python_files
-    python_bindings.cc)
+    corr_est_cc_python.cc python_bindings.cc)
 
 GR_PYBIND_MAKE_OOT(elen90089
    ../..

python/bindings/corr_est_cc_python.cc

+/*
+ * Copyright 2022 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ *
+ */
+
+/***********************************************************************************/
+/* This file is automatically generated using bindtool and can be manually edited  */
+/* The following lines can be configured to regenerate this file during cmake      */
+/* If manual edits are made, the following tags should be modified accordingly.    */
+/* BINDTOOL_GEN_AUTOMATIC(0)                                                       */
+/* BINDTOOL_USE_PYGCCXML(0)                                                        */
+/* BINDTOOL_HEADER_FILE(corr_est_cc.h)                                        */
+/* BINDTOOL_HEADER_FILE_HASH(53737d29a95e3a8c008b00324c083e88)                     */
+/***********************************************************************************/
+
+#include <pybind11/complex.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+namespace py = pybind11;
+
+#include <elen90089/corr_est_cc.h>
+// pydoc.h is automatically generated in the build directory
+#include <corr_est_cc_pydoc.h>
+
+void bind_corr_est_cc(py::module& m)
+{
+    using corr_est_cc = ::gr::elen90089::corr_est_cc;
+
+    py::class_<corr_est_cc,
+               gr::sync_block,
+               gr::block,
+               gr::basic_block,
+               std::shared_ptr<corr_est_cc>>(m, "corr_est_cc", D(corr_est_cc))
+
+        .def(py::init(&corr_est_cc::make),
+             py::arg("sequence"),
+             py::arg("threshold") = 0.5,
+             D(corr_est_cc, make))
+
+        .def("sequence",
+             &corr_est_cc::sequence,       
+             D(corr_est_cc, sequence))
+
+        .def("set_sequence",
+             &corr_est_cc::set_sequence,       
+             py::arg("sequence"),
+             D(corr_est_cc, set_sequence))
+
+        .def("threshold",
+             &corr_est_cc::threshold,       
+             D(corr_est_cc, threshold))
+
+        .def("set_threshold",
+             &corr_est_cc::set_threshold,       
+             py::arg("threshold"),
+             D(corr_est_cc, set_threshold));
+
+}
+

python/bindings/docstrings/corr_est_cc_pydoc_template.h

+/*
+ * Copyright 2022 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ *
+ */
+#include "pydoc_macros.h"
+#define D(...) DOC(gr,elen90089, __VA_ARGS__ )
+/*
+  This file contains placeholders for docstrings for the Python bindings.
+  Do not edit! These were automatically extracted during the binding process
+  and will be overwritten during the build process
+ */
+
+
+ 
+ static const char *__doc_gr_elen90089_corr_est_cc = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_corr_est_cc_corr_est_cc_0 = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_corr_est_cc_corr_est_cc_1 = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_corr_est_cc_make = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_corr_est_cc_sequence = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_corr_est_cc_set_sequence = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_corr_est_cc_threshold = R"doc()doc";
+
+
+ static const char *__doc_gr_elen90089_corr_est_cc_set_threshold = R"doc()doc";
+
+  

python/bindings/python_bindings.cc

@@ -21,6 +21,7 @@ namespace py = pybind11;
 // Please do not delete
 /**************************************/
 // BINDING_FUNCTION_PROTOTYPES(
+    void bind_corr_est_cc(py::module& m);
 // ) END BINDING_FUNCTION_PROTOTYPES
 
 
@@ -49,5 +50,6 @@ PYBIND11_MODULE(elen90089_python, m)
     // Please do not delete
     /**************************************/
     // BINDING_FUNCTION_CALLS(
+    bind_corr_est_cc(m);
     // ) END BINDING_FUNCTION_CALLS
 }
\ No newline at end of file

python/qa_corr_est_cc.py

+from gnuradio import gr, gr_unittest
+from gnuradio import blocks
+import pmt
+import elen90089
+import numpy as np
+
+
+class qa_corr_est_cc(gr_unittest.TestCase):
+
+    def setUp(self):
+        self.tb = gr.top_block()
+
+    def tearDown(self):
+        self.tb = None
+
+    def test_001(self):
+        """Test correlation result"""
+        pad = (0,) * 16
+        code = (1, 0, 1, 1, 1, 0, 1, 1)
+        code_sym = [(2.0*x - 1.0) for x in code] 
+        src_bits = pad + code + pad 
+        src_data = [(2.0*x - 1.0) for x in src_bits]
+        # build flowgraph
+        src = blocks.vector_source_c(src_data)
+        cor = elen90089.corr_est_cc(code_sym, threshold=0.5)
+        dst = blocks.tag_debug(gr.sizeof_gr_complex, "", "corr_start")
+        dst.set_display(False)
+        self.tb.connect(src, cor, dst)
+        self.tb.run()
+        # check test results
+        result_data = dst.current_tags()
+        index = len(pad) + len(code) - 1
+        self.assertEqual(len(result_data), 1)
+        self.assertEqual(result_data[0].offset, index)
+        corr_mag = pmt.to_python(result_data[0].value)
+        self.assertAlmostEqual(corr_mag, 1.0, places=6)
+
+    def test_002(self):
+        """Test coarse frequency offset estimate."""
+        phase_inc = 2*np.pi*0.02 # freq offset
+        pad = (0,) * 16
+        code = (1, 0, 1, 1, 1, 0, 1, 1)
+        code_sym = [(2.0*x - 1.0) for x in code] 
+        src_bits = pad + code + pad 
+        src_data = [(2.0*x - 1.0) for x in src_bits]
+        # build flowgraph
+        src = blocks.vector_source_c(src_data)
+        rot = blocks.rotator_cc(phase_inc)
+        cor = elen90089.corr_est_cc(code_sym, threshold=0.5)
+        dst = blocks.tag_debug(gr.sizeof_gr_complex, "", "freq_est")
+        dst.set_display(False)
+        self.tb.connect(src, rot, cor, dst)
+        self.tb.run()
+        # check test results
+        result_data = dst.current_tags()
+        index = len(pad) + len(code) - 1
+        self.assertEqual(len(result_data), 1)
+        self.assertEqual(result_data[0].offset, index)
+        freq_est = pmt.to_python(result_data[0].value)
+        self.assertAlmostEqual(freq_est, phase_inc, places=6)
+
+if __name__ == '__main__':
+    gr_unittest.run(qa_corr_est_cc)