0001-v0.0.18-adding_sgd_algo.patch

From 50d75b8e8f10de9c6b74d4c9f63c27045fc84bfe Mon Sep 17 00:00:00 2001
From: Gwenhael Goavec-Merou <gwenhael.goavec-merou@trabucayre.com>
Date: Fri, 28 Jul 2023 15:43:45 +0200
Subject: [PATCH] adding_sgd_algo

Signed-off-by: Gwenhael Goavec-Merou <gwenhael.goavec-merou@trabucayre.com>
---
 .../adapters/uhd_signal_source.cc             |  27 +-
 .../adapters/uhd_signal_source.h              |   2 +
 .../signal_source/libs/CMakeLists.txt         |   2 +
 src/algorithms/signal_source/libs/sgd.h       | 101 ++++++++
 src/algorithms/signal_source/libs/sgd_impl.cc | 237 ++++++++++++++++++
 5 files changed, 368 insertions(+), 1 deletion(-)
 create mode 100644 src/algorithms/signal_source/libs/sgd.h
 create mode 100644 src/algorithms/signal_source/libs/sgd_impl.cc

diff --git a/src/algorithms/signal_source/adapters/uhd_signal_source.cc b/src/algorithms/signal_source/adapters/uhd_signal_source.cc
index efaae370b..2a738abb9 100644
--- a/src/algorithms/signal_source/adapters/uhd_signal_source.cc
+++ b/src/algorithms/signal_source/adapters/uhd_signal_source.cc
@@ -21,6 +21,7 @@
 #include "gnss_sdr_filesystem.h"
 #include "gnss_sdr_string_literals.h"
 #include "gnss_sdr_valve.h"
+#include "sgd.h"
 #include <glog/logging.h>
 #include <uhd/exception.hpp>
 #include <uhd/types/device_addr.hpp>
@@ -58,10 +59,25 @@ UhdSignalSource::UhdSignalSource(const ConfigurationInterface* configuration,
         {
             dev_addr["serial"] = device_serial;
         }
-    subdevice_ = configuration->property(role + ".subdevice", empty);
     clock_source_ = configuration->property(role + ".clock_source", std::string("internal"));
     otw_format_ = configuration->property(role + ".otw_format", std::string("sc16"));
     RF_channels_ = configuration->property(role + ".RF_channels", 1);
+    sgd_  = configuration->property(role + ".sgd", 0);
+    if (sgd_ !=0)
+        {
+            printf("SGD jamming protection %d\n",sgd_); fflush(stdout);
+            RF_channels_=sgd_; // SGD jamming protection: override RF_channels in this block only => N inputs and RF_chan=1 output
+            subdevice_ = configuration->property(role + ".subdevice", std::string("A:A A:B"));
+            float sgd_alpha = configuration->property(role + ".sgd_alpha", 1e-2);
+            bool sgd_mean = configuration->property(role + ".sgd_mean", true);
+            int sgd_mean_length = configuration->property(role + ".sgd_mean_length", 100);
+            int sgd_iter_count = configuration->property(role + ".sgd_iter_count", 10000);
+            jamming_sgd_=gnss_sdr_make_sgd(0, 5e-3, sgd_alpha, sgd_mean, sgd_mean_length, sgd_iter_count);
+        }
+    if ((sgd_ == 0))
+        {
+            subdevice_ = configuration->property(role + ".subdevice", empty);
+        }
     sample_rate_ = configuration->property(role + ".sampling_frequency", 4.0e6);
     item_type_ = configuration->property(role + ".item_type", default_item_type);
 
@@ -315,6 +331,11 @@ void UhdSignalSource::connect(gr::top_block_sptr top_block)
                             DLOG(INFO) << "connected usrp source to file sink RF Channel " << i;
                         }
                 }
+            if (sgd_ != 0)
+                {
+                    top_block->connect(uhd_source_, i, jamming_sgd_, i);
+                    printf("UHD -> SGD connect: %d\n",i);fflush(stdout);
+                }
         }
 }
 
@@ -365,5 +386,9 @@ gr::basic_block_sptr UhdSignalSource::get_right_block(int RF_channel)
         {
             return valve_.at(RF_channel);
         }
+    if ( jamming_sgd_ != 0ULL)
+        {
+            return jamming_sgd_;
+        }
     return uhd_source_;
 }
diff --git a/src/algorithms/signal_source/adapters/uhd_signal_source.h b/src/algorithms/signal_source/adapters/uhd_signal_source.h
index 5a513b4c9..d3f1e3a8a 100644
--- a/src/algorithms/signal_source/adapters/uhd_signal_source.h
+++ b/src/algorithms/signal_source/adapters/uhd_signal_source.h
@@ -60,6 +60,7 @@ public:
 
 private:
     gr::uhd::usrp_source::sptr uhd_source_;
+    gnss_shared_ptr<gr::block> jamming_sgd_;
 
     std::vector<gnss_shared_ptr<gr::block>> valve_;
     std::vector<gr::blocks::file_sink::sptr> file_sink_;
@@ -87,6 +88,7 @@ private:
     double sample_rate_;
     size_t item_size_;
     int RF_channels_;
+    int sgd_;
     unsigned int in_stream_;
     unsigned int out_stream_;
 };
diff --git a/src/algorithms/signal_source/libs/CMakeLists.txt b/src/algorithms/signal_source/libs/CMakeLists.txt
index b4db8353c..57b094456 100644
--- a/src/algorithms/signal_source/libs/CMakeLists.txt
+++ b/src/algorithms/signal_source/libs/CMakeLists.txt
@@ -32,6 +32,7 @@ set(SIGNAL_SOURCE_LIB_SOURCES
     rtl_tcp_commands.cc
     rtl_tcp_dongle_info.cc
     gnss_sdr_valve.cc
+    sgd_impl.cc
     gnss_sdr_timestamp.cc
     ${OPT_SIGNAL_SOURCE_LIB_SOURCES}
 )
@@ -40,6 +41,7 @@ set(SIGNAL_SOURCE_LIB_HEADERS
     rtl_tcp_commands.h
     rtl_tcp_dongle_info.h
     gnss_sdr_valve.h
+    sgd.h
     ${OPT_SIGNAL_SOURCE_LIB_HEADERS}
 )
 
diff --git a/src/algorithms/signal_source/libs/sgd.h b/src/algorithms/signal_source/libs/sgd.h
new file mode 100644
index 000000000..f9b7fef5d
--- /dev/null
+++ b/src/algorithms/signal_source/libs/sgd.h
@@ -0,0 +1,101 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2020 gr-sgd author.
+ *
+ * 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_SGD_SGD_IMPL_H
+#define INCLUDED_SGD_SGD_IMPL_H
+#if GNURADIO_USES_STD_POINTERS
+#include <memory>
+#else
+#include <boost/shared_ptr.hpp>
+#endif
+#include <gnuradio/sync_block.h>  // for sync_block
+#include <gnuradio/types.h>       // for gr_vector_const_void_star
+#include <pmt/pmt.h>
+
+#include <volk/volk.h>
+
+class sgd_impl;
+
+#if GNURADIO_USES_STD_POINTERS
+std::shared_ptr<sgd_impl> gnss_sdr_make_sgd(
+    int delay_max, float seuil, float alpha,
+        bool mean, int mean_length, int iter_count);
+#else
+boost::shared_ptr<sgd_impl> gnss_sdr_make_sgd(
+    int delay_max, float seuil, float alpha,
+        bool mean, int mean_length, int iter_count);
+#endif
+
+    class sgd_impl : public gr::sync_block
+    {
+     private:
+#if GNURADIO_USES_STD_POINTERS
+         friend std::shared_ptr<sgd_impl> gnss_sdr_make_sgd(
+             int delay_max, float seuil, float alpha,
+                  bool mean, int mean_length, int iter_count);
+#else
+         friend boost::shared_ptr<sgd_impl> gnss_sdr_make_sgd(
+             int delay_max, float seuil, float alpha,
+                  bool mean, int mean_length, int iter_count);
+#endif
+
+      int _w1_size;
+      float _seuil;
+      float _alpha;
+      int _delay_max;
+      uint32_t alignment;
+
+      gr_complex *tmp2;
+      gr_complex e;
+      float val_max;
+      uint32_t *val_max_index;
+      gr_complex *XXxw1;
+      gr_complex *w1;
+      float *w1_mag;
+      gr_complex *xxConj;
+      int _residual;
+      FILE *_w1_out;
+      int _iter;
+      std::complex<double> **_w1_array;
+      std::complex<double> *w1_accum;
+      gr_complex *w1_res;
+      int _array_index;
+      int _nb_accum;
+      bool _mean;
+      int _mean_length;
+      int _iter_count;
+
+     public:
+      sgd_impl();
+      sgd_impl(int w1_size, float seuil, float alpha,
+                bool mean, int mean_length, int iter_count);
+      ~sgd_impl();
+      int fixed_rate_ninput_to_noutput(int ninput);
+
+      // Where all the action really happens
+      int work(
+              int noutput_items,
+              gr_vector_const_void_star &input_items,
+              gr_vector_void_star &output_items
+      );
+    };
+
+#endif /* INCLUDED_SGD_SGD_IMPL_H */
+
diff --git a/src/algorithms/signal_source/libs/sgd_impl.cc b/src/algorithms/signal_source/libs/sgd_impl.cc
new file mode 100644
index 000000000..f83da8948
--- /dev/null
+++ b/src/algorithms/signal_source/libs/sgd_impl.cc
@@ -0,0 +1,237 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2020 gr-sgd author.
+ *
+ * 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 "sgd.h"
+
+//#define DEBUG_COEFF
+
+#if GNURADIO_USES_STD_POINTERS
+std::shared_ptr<sgd_impl> gnss_sdr_make_sgd(
+    int delay_max, float seuil, float alpha, bool mean, int mean_length,
+        int iter_count)
+{
+	std::shared_ptr<sgd_impl> sgd_inst(new sgd_impl(delay_max, seuil, alpha, mean, mean_length, iter_count));
+	return sgd_inst;
+}
+#else
+boost::shared_ptr<sgd_impl> gnss_sdr_make_sgd(
+    int delay_max, float seuil, float alpha, bool mean, int mean_length,
+        int iter_count)
+{
+	boost::shared_ptr<sgd_impl> sgd_inst(new sgd_impl(delay_max, seuil, alpha, mean, mean_length, iter_count));
+	return sgd_inst;
+}
+#endif
+
+    /*
+     * The private constructor
+     */
+    sgd_impl::sgd_impl(int delay_max, float seuil, float alpha, bool mean, int mean_length, int iter_count)
+      : gr::sync_block("sgd",
+              gr::io_signature::make(2, 2, sizeof(gr_complex)),
+              gr::io_signature::make(1, 1, sizeof(gr_complex))),
+              _w1_size(delay_max * 2 + 1), _seuil(seuil), _alpha(alpha),
+              _delay_max(delay_max), _residual(0), _w1_out(NULL), _iter(1), 
+                          _array_index(0), _nb_accum(1), _mean(mean), _mean_length(mean_length),
+                          _iter_count(iter_count)
+    {
+        std::cout << "SGD sliding average: "<< _mean << " on " << _mean_length << "samples" << std::endl;
+        alignment = volk_get_alignment();
+        tmp2   = (gr_complex *)volk_malloc(sizeof(gr_complex) * _w1_size, alignment);
+        val_max_index = (uint32_t *)volk_malloc(sizeof(uint32_t), alignment);
+        XXxw1  = (gr_complex *)volk_malloc(sizeof(gr_complex), alignment);
+        w1     = (gr_complex *)volk_malloc(sizeof(gr_complex) * _w1_size, alignment);
+        w1_mag = (float *)volk_malloc(sizeof(float)* _w1_size, alignment);
+        xxConj = (gr_complex *)volk_malloc(sizeof(gr_complex) * _w1_size, alignment);
+
+        w1_res = (gr_complex *)volk_malloc(sizeof(gr_complex) * _w1_size, alignment);
+        if (_mean) {
+           w1_accum = (std::complex<double> *)malloc(sizeof(std::complex<double>) * _w1_size);
+           _w1_array = (std::complex<double> **)malloc(sizeof(std::complex<double>*) * _mean_length);
+           for (int i=0; i < _mean_length; i++) {
+               _w1_array[i] = (std::complex<double> *)malloc(sizeof(std::complex<double>) * _w1_size);
+               for (int ii=0; ii < _w1_size; ii++) 
+                   _w1_array[i][ii]={0.0};
+              }
+           for (int ii=0; ii < _w1_size; ii++)
+              w1_accum[ii]={0.,0.};
+          }
+
+        /* cleanup w1 */
+        for (int i=0; i < _w1_size; i++)
+            w1[i] = 0;
+
+//#ifdef DEBUG_COEFF
+	_w1_out = fopen("w1_dump.bin", "w+");
+//#endif
+	}
+
+    /*
+     * Our virtual destructor.
+     */
+    sgd_impl::~sgd_impl()
+    {
+        for (int index = 0; index < _w1_size; index++) {
+			float tr = w1[index].real();
+			float ti = w1[index].imag();
+			fwrite(&tr, sizeof(float), 1, _w1_out);
+			fwrite(&ti, sizeof(float), 1, _w1_out);
+		}
+//#ifdef DEBUG_COEFF
+		fclose(_w1_out);
+//#endif
+        volk_free(tmp2);
+        volk_free(val_max_index);
+        volk_free(XXxw1);
+        volk_free(w1);
+        volk_free(w1_mag);
+        volk_free(xxConj);
+        volk_free(w1_res);
+        if (_mean) {
+           free(w1_accum);
+           for (int i = 0; i < _mean_length; i++)
+               free(_w1_array[i]);
+           free(_w1_array);
+          }
+    }
+
+    int
+    sgd_impl::fixed_rate_ninput_to_noutput(int ninput)  
+    {
+      return ninput - (_w1_size);
+    }
+
+    int
+    sgd_impl::work(int noutput_items,
+        gr_vector_const_void_star &input_items,
+        gr_vector_void_star &output_items)
+    {
+      const gr_complex *x = (const gr_complex *) input_items[0];
+      const gr_complex *y = (const gr_complex *) input_items[1];
+      gr_complex *res = (gr_complex *) output_items[0];
+
+      int size = noutput_items;
+
+	  if (size < _w1_size) {
+		  _residual = size;
+		  set_history(_residual);
+		  return 0;
+	  }
+
+      for (int k=0; k < size-_w1_size; k++) {
+
+        volk_32fc_x2_dot_prod_32fc(XXxw1, x+k, w1, _w1_size);
+        //e = (y[k + _delay_max] - *XXxw1) * (float)_alpha;//(1/sqrtf(_iter));
+        //e = (y[k + _delay_max] - *XXxw1) * (float)(1./sqrtf((float)_iter));  // loss when moving with average
+        e = (y[k + _delay_max] - *XXxw1) * (float)(_alpha/sqrtf((float)_iter));  // loss when moving with average
+                if (_iter < _iter_count)
+                        _iter++;
+                else {  
+                        _iter = 1;
+                        //for (int index = 0; index < _w1_size; index ++)
+                        //      std::cout << index - (_delay_max) << " " << w1[index] << std::endl;
+		}
+
+        volk_32fc_conjugate_32fc(xxConj, x+k, _w1_size);
+        volk_32fc_s32fc_multiply_32fc(tmp2, xxConj, e, _w1_size);
+
+        volk_32fc_x2_add_32fc(w1, w1, tmp2, _w1_size);
+
+// remove if _w1_size == 1 (case of GPS un-jamming)
+        if (_w1_size>1)
+         {	
+	        /* abs */
+	        volk_32fc_magnitude_32f(w1_mag, w1, _w1_size);
+	        /* search max */
+	        volk_32f_index_max_32u(val_max_index, w1_mag, _w1_size);
+	        /* store max */
+	        val_max = w1_mag[*val_max_index];
+	
+	        val_max *= _seuil;
+	        /* all entry < val_max => 0 */
+	        for (int index = 0; index < _w1_size; index++) {
+            		if (w1_mag[index] < val_max)
+                		w1[index] = 0;
+#ifdef DEBUG_COEFF
+			float toto = w1[index].real();
+			fwrite(&toto, sizeof(float), 1, _w1_out);
+			toto = w1[index].imag();
+			fwrite(&toto, sizeof(float), 1, _w1_out);
+#endif
+		}
+         }
+// remove until here if _w1_size == 1 (case of GPS un-jamming)
+        if (_mean) {
+           for (int index = 0; index < _w1_size; index++) {
+               w1_accum[index] -= _w1_array[_array_index][index];
+
+               _w1_array[_array_index][index].real((double)w1[index].real());
+               _w1_array[_array_index][index].imag((double)w1[index].imag());
+
+               w1_accum[index] += _w1_array[_array_index][index];
+
+               std::complex<double> t = w1_accum[index] / (double)_nb_accum;
+               w1_res[index].real((float)t.real());
+               w1_res[index].imag((float)t.imag());
+              }
+
+
+           if (_array_index == _mean_length-1)
+               _array_index = 0;
+           else
+               _array_index++;
+                if (_nb_accum < _mean_length) _nb_accum++;
+         } else {
+                memcpy(w1_res, w1, _w1_size * sizeof(gr_complex));
+               }
+        if (_iter == 1) {
+           for (int index = 0; index < _w1_size; index ++) {
+               for (int index = 0; index < _w1_size; index ++) {
+                   std::cout << index - (_delay_max) << " ";
+                   std::cout << w1_res[index].real() << " " << w1_res[index].imag() << std::endl;
+                  }
+              }
+         }
+      }
+
+      /* suppress part */
+      int maxsize = size;
+      int coeffsize = _w1_size;
+      int sum_size = maxsize-coeffsize;
+      gr_complex tmp;
+      for (int i=0; i < sum_size; i++) {
+          tmp = 0;
+          volk_32fc_x2_dot_prod_32fc(&tmp, x+i, w1_res, coeffsize);
+          res[i] = y[_delay_max + i] - tmp;
+      }
+
+      _residual = _w1_size - 1;
+	  set_history(_residual);
+
+      // Tell runtime system how many output items we produced.
+      return sum_size;
+    }
+
+
-- 
2.41.0