add correlator block

grc/CMakeLists.txt

@@ -19,5 +19,5 @@
 install(FILES
     ccsds_ccsds_encoder.xml
     ccsds_ccsds_decoder.xml
-    DESTINATION share/gnuradio/grc/blocks
+    ccsds_correlator.xml DESTINATION share/gnuradio/grc/blocks
 )

grc/ccsds_correlator.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<block>
+  <name>Correlator</name>
+  <key>ccsds_correlator</key>
+  <category>[CCSDS]</category>
+  <import>import ccsds</import>
+  <make>ccsds.correlator($asm, $asm_mask, $threshold, $frame_len)</make>
+  <param>
+    <name>ASM</name>
+    <key>asm</key>
+    <value>0x1acffc1d</value>
+    <type>int</type>
+  </param>
+  <param>
+    <name>ASM Mask</name>
+    <key>asm_mask</key>
+    <value>0xffffffff</value>
+    <type>int</type>
+  </param>
+  <param>
+    <name>Threshold Mask</name>
+    <key>threshold</key>
+    <value>0</value>
+    <type>int</type>
+  </param>
+  <param>
+    <name>Frame Length</name>
+    <key>frame_len</key>
+    <value>223</value>
+    <type>int</type>
+  </param>
+  <sink>
+    <name>in</name>
+    <type>byte</type>
+  </sink>
+  <source>
+    <name>out</name>
+    <type>message</type>
+  </source>
+</block>

include/ccsds/CMakeLists.txt

@@ -24,5 +24,5 @@ install(FILES
     api.h
     ccsds_encoder.h
     ccsds_decoder.h
-    DESTINATION include/ccsds
+    correlator.h DESTINATION include/ccsds
 )

include/ccsds/correlator.h

@@ -0,0 +1,60 @@
+/* -*- c++ -*- */
+/* 
+ * Copyright 2018 André Løfaldli.
+ * 
+ * This 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.
+ * 
+ * This software 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 this software; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+
+#ifndef INCLUDED_CCSDS_CORRELATOR_H
+#define INCLUDED_CCSDS_CORRELATOR_H
+
+#include <ccsds/api.h>
+#include <gnuradio/sync_block.h>
+
+namespace gr {
+  namespace ccsds {
+
+    /*!
+     * \brief CCSDS correlator
+     * \ingroup ccsds
+     *
+     * looks for the attached sync marker and produces pdus containing frames
+     */
+    class CCSDS_API correlator : virtual public gr::sync_block
+    {
+     public:
+      typedef boost::shared_ptr<correlator> sptr;
+
+      /*!
+       * \brief make the correlator.
+       *
+       * \param asm attached sync marker
+       * \param asm_mask mask for attached sync marker
+       * \param threshold maximum number of allowed errors in asm
+       * \param frame_len length of the transfer frame
+       */
+      static sptr make(const uint64_t asm_=0x1acffc1d,
+                       const uint64_t asm_mask=0xffffffff, 
+                       const uint8_t threshold=2, 
+                       const size_t frame_len=223);
+    };
+
+  } // namespace ccsds
+} // namespace gr
+
+#endif /* INCLUDED_CCSDS_CORRELATOR_H */
+

lib/CMakeLists.txt

@@ -33,7 +33,7 @@ list(APPEND ccsds_sources
     reed_solomon.cc
     ccsds_encoder_impl.cc
     ccsds_decoder_impl.cc
-    )
+    correlator_impl.cc )
 
 set(ccsds_sources "${ccsds_sources}" PARENT_SCOPE)
 if(NOT ccsds_sources)

lib/correlator_impl.cc

@@ -0,0 +1,148 @@
+/* -*- c++ -*- */
+/* 
+ * Copyright 2018 André Løfaldli.
+ * 
+ * This 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.
+ * 
+ * This software 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 this software; 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 <gnuradio/io_signature.h>
+#include <volk/volk.h>
+#include "correlator_impl.h"
+
+#if 0
+#define debug_print printf
+#else
+#define debug_print
+#endif
+
+namespace gr {
+  namespace ccsds {
+
+    correlator::sptr
+    correlator::make(const uint64_t asm_, const uint64_t asm_mask,
+                     const uint8_t threshold, const size_t frame_len)
+    {
+      return gnuradio::get_initial_sptr
+        (new correlator_impl(asm_, asm_mask, threshold, frame_len));
+    }
+
+    correlator_impl::correlator_impl(const uint64_t asm_, const uint64_t asm_mask,
+                                     const uint8_t threshold, const size_t frame_len)
+      : gr::sync_block("correlator",
+              gr::io_signature::make(1, 1, sizeof(uint8_t)),
+              gr::io_signature::make(0, 0, 0)),
+      d_asm(asm_), d_asm_mask(asm_mask),
+      d_threshold(threshold), d_frame_len(frame_len),
+      d_frame_count(0), d_ambiguity(NONE)
+    {
+        message_port_register_out(pmt::mp("out"));
+        d_frame_buffer = (uint8_t *)malloc(frame_len * sizeof(uint8_t));
+        enter_state(SEARCH);
+    }
+
+    correlator_impl::~correlator_impl() {
+        free(d_frame_buffer);
+    }
+
+    int
+    correlator_impl::work(int noutput_items,
+        gr_vector_const_void_star &input_items,
+        gr_vector_void_star &output_items)
+    {
+        const uint8_t *in = (const uint8_t *) input_items[0];
+
+        uint64_t count = 0;
+        while (count < noutput_items) {
+            switch (d_state) {
+            case SEARCH:
+                d_asm_buf = (d_asm_buf << 1) | (in[count++] & 0x01);
+                if (check_asm(d_asm_buf)) {
+                    d_ambiguity = NONE;
+                    enter_state(LOCK);
+                } else if (check_asm(d_asm_buf ^ 0xffffffffffffffff)) {
+                    d_ambiguity = INVERTED;
+                    enter_state(LOCK);
+                }
+                break;
+            case LOCK:
+                d_byte_buf = (d_byte_buf << 1) | (in[count++] & 0x01);
+                d_bit_ctr++;
+                if (d_bit_ctr == 8) {
+                    if (d_ambiguity == NONE) {
+                        d_frame_buffer[d_frame_buffer_len] = d_byte_buf;
+                    } else if (d_ambiguity == INVERTED) {
+                        d_frame_buffer[d_frame_buffer_len] = d_byte_buf ^ 0xff;
+                    }
+                    d_frame_buffer_len++;
+                    d_bit_ctr = 0;
+                }
+                if (d_frame_buffer_len == d_frame_len) {
+                    publish_msg();
+                    d_frame_count++;
+                    enter_state(SEARCH);
+                }
+                break;
+            }
+        }
+        return noutput_items;
+    }
+
+    bool correlator_impl::check_asm(const uint64_t asm_buf) {
+        debug_print("check_asm\n");
+
+        uint64_t nerrors = 0;
+        const uint64_t syndrome = (asm_buf ^ d_asm) & d_asm_mask;
+        volk_64u_popcnt(&nerrors, syndrome);
+        return nerrors <= (uint64_t)d_threshold;
+    }
+
+    void correlator_impl::enter_state(const state_t state) {
+        debug_print("enter_state ");
+
+        switch (state) {
+        case SEARCH:
+            debug_print("SEARCH\n");
+
+            d_asm_buf = 0;
+            break;
+        case LOCK:
+            debug_print("LOCK\n");
+
+            d_byte_buf = 0;
+            d_bit_ctr = 0;
+            d_frame_buffer_len = 0;
+            break;
+        }
+        d_state = state;
+    }
+
+    void correlator_impl::publish_msg() {
+        debug_print("publish_msg #%lu\n", d_frame_count);
+
+        pmt::pmt_t meta = pmt::make_dict();
+        meta = dict_add(meta, pmt::intern("frame_count"), 
+                        pmt::from_uint64(d_frame_count));
+        const pmt::pmt_t data = pmt::init_u8vector(d_frame_len, d_frame_buffer);
+        message_port_pub(pmt::mp("out"), pmt::cons(meta, data));
+    }
+
+  } /* namespace ccsds */
+} /* namespace gr */
+

lib/correlator_impl.h

@@ -0,0 +1,65 @@
+/* -*- c++ -*- */
+/* 
+ * Copyright 2018 André Løfaldli.
+ * 
+ * This 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.
+ * 
+ * This software 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 this software; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifndef INCLUDED_CCSDS_CORRELATOR_IMPL_H
+#define INCLUDED_CCSDS_CORRELATOR_IMPL_H
+
+#include <ccsds/correlator.h>
+#include <vector>
+
+namespace gr {
+  namespace ccsds {
+
+    enum state_t { SEARCH, LOCK };
+    enum ambiguity_t { NONE, INVERTED };
+
+    class correlator_impl : public correlator {
+      private:
+        const uint64_t d_asm, d_asm_mask;
+        const uint8_t d_threshold;
+        const size_t d_frame_len;
+
+        uint64_t d_asm_buf;
+        uint8_t *d_frame_buffer;
+        size_t d_frame_buffer_len;
+        uint8_t d_bit_ctr, d_byte_buf;
+
+        state_t d_state;
+        ambiguity_t d_ambiguity;
+        uint64_t d_frame_count;
+
+      public:
+        correlator_impl(const uint64_t asm_, const uint64_t asm_mask, 
+                        const uint8_t threshold, const size_t frame_len);
+        ~correlator_impl(); 
+        int work(int noutput_items,
+           gr_vector_const_void_star &input_items,
+           gr_vector_void_star &output_items);
+
+        bool check_asm(const uint64_t asm_buf);
+        void enter_state(const state_t state);
+        void publish_msg();
+    };
+
+  } // namespace ccsds
+} // namespace gr
+
+#endif /* INCLUDED_CCSDS_CORRELATOR_IMPL_H */
+

python/CMakeLists.txt

@@ -43,3 +43,4 @@ set(GR_TEST_TARGET_DEPS gnuradio-ccsds)
 set(GR_TEST_PYTHON_DIRS ${CMAKE_BINARY_DIR}/swig)
 GR_ADD_TEST(qa_ccsds_encoder ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/qa_ccsds_encoder.py)
 GR_ADD_TEST(qa_ccsds_decoder ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/qa_ccsds_decoder.py)
+GR_ADD_TEST(qa_correlator ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/qa_correlator.py)