From cd9784efb659b3d44c597badfef0e2924f2ad6b6 Mon Sep 17 00:00:00 2001 From: Thomas Kolb Date: Tue, 23 Jul 2019 23:16:48 +0200 Subject: [PATCH] Added frequency estimator blocks - correct_frequency: Correct frequency by averaging the phase drift on the preamble - freq_est_lr: Luise & Reggiannini frequency estimator - pid_controller: a PID controller that takes error values at the input and generates steering values at the output The combination of freq_est_lr and PID controller can be used together with an NCO (signal source) to create a closed control loop for frequency offset compensation. --- gr-hamnet70/CMakeLists.txt | 2 +- gr-hamnet70/grc/CMakeLists.txt | 6 +- .../grc/hamnet70_correct_frequency.xml | 25 ++++ gr-hamnet70/grc/hamnet70_freq_est_lr.xml | 37 +++++ gr-hamnet70/grc/hamnet70_pid_controller.xml | 53 +++++++ gr-hamnet70/include/hamnet70/CMakeLists.txt | 5 +- .../include/hamnet70/correct_frequency.h | 56 +++++++ gr-hamnet70/include/hamnet70/freq_est_lr.h | 56 +++++++ gr-hamnet70/include/hamnet70/pid_controller.h | 56 +++++++ gr-hamnet70/lib/CMakeLists.txt | 4 +- gr-hamnet70/lib/correct_frequency_impl.cc | 134 +++++++++++++++++ gr-hamnet70/lib/correct_frequency_impl.h | 57 +++++++ .../lib/correct_phase_from_tag_impl.cc | 4 - gr-hamnet70/lib/freq_est_lr_impl.cc | 141 ++++++++++++++++++ gr-hamnet70/lib/freq_est_lr_impl.h | 54 +++++++ gr-hamnet70/lib/pid_controller_impl.cc | 99 ++++++++++++ gr-hamnet70/lib/pid_controller_impl.h | 58 +++++++ gr-hamnet70/make.sh | 6 + gr-hamnet70/python/CMakeLists.txt | 3 + gr-hamnet70/python/qa_correct_frequency.py | 41 +++++ gr-hamnet70/python/qa_freq_est_lr.py | 41 +++++ gr-hamnet70/python/qa_pid_controller.py | 41 +++++ gr-hamnet70/swig/hamnet70_swig.i | 10 ++ 23 files changed, 980 insertions(+), 9 deletions(-) create mode 100644 gr-hamnet70/grc/hamnet70_correct_frequency.xml create mode 100644 gr-hamnet70/grc/hamnet70_freq_est_lr.xml create mode 100644 gr-hamnet70/grc/hamnet70_pid_controller.xml create mode 100644 gr-hamnet70/include/hamnet70/correct_frequency.h create mode 100644 gr-hamnet70/include/hamnet70/freq_est_lr.h create mode 100644 gr-hamnet70/include/hamnet70/pid_controller.h create mode 100644 gr-hamnet70/lib/correct_frequency_impl.cc create mode 100644 gr-hamnet70/lib/correct_frequency_impl.h create mode 100644 gr-hamnet70/lib/freq_est_lr_impl.cc create mode 100644 gr-hamnet70/lib/freq_est_lr_impl.h create mode 100644 gr-hamnet70/lib/pid_controller_impl.cc create mode 100644 gr-hamnet70/lib/pid_controller_impl.h create mode 100755 gr-hamnet70/python/qa_correct_frequency.py create mode 100755 gr-hamnet70/python/qa_freq_est_lr.py create mode 100755 gr-hamnet70/python/qa_pid_controller.py diff --git a/gr-hamnet70/CMakeLists.txt b/gr-hamnet70/CMakeLists.txt index 04d9e0f..6ae4660 100644 --- a/gr-hamnet70/CMakeLists.txt +++ b/gr-hamnet70/CMakeLists.txt @@ -140,7 +140,7 @@ find_package(Doxygen) # components required to the list of GR_REQUIRED_COMPONENTS (in all # caps such as FILTER or FFT) and change the version to the minimum # API compatible version required. -set(GR_REQUIRED_COMPONENTS RUNTIME) +set(GR_REQUIRED_COMPONENTS RUNTIME FILTER) find_package(Gnuradio "3.7.2" REQUIRED) list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake/Modules) include(GrVersion) diff --git a/gr-hamnet70/grc/CMakeLists.txt b/gr-hamnet70/grc/CMakeLists.txt index ad070e2..df31b4d 100644 --- a/gr-hamnet70/grc/CMakeLists.txt +++ b/gr-hamnet70/grc/CMakeLists.txt @@ -17,7 +17,9 @@ # 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. - install(FILES - hamnet70_correct_phase_from_tag.xml DESTINATION share/gnuradio/grc/blocks + hamnet70_correct_phase_from_tag.xml + hamnet70_correct_frequency.xml + hamnet70_freq_est_lr.xml + hamnet70_pid_controller.xml DESTINATION share/gnuradio/grc/blocks ) diff --git a/gr-hamnet70/grc/hamnet70_correct_frequency.xml b/gr-hamnet70/grc/hamnet70_correct_frequency.xml new file mode 100644 index 0000000..c26acec --- /dev/null +++ b/gr-hamnet70/grc/hamnet70_correct_frequency.xml @@ -0,0 +1,25 @@ + + + Correct Frequency + hamnet70_correct_frequency + [hamnet70] + import hamnet70 + hamnet70.correct_frequency($symbols) + + + Symbols + symbols + complex_vector + + + + in + complex + + + + out + complex + + + diff --git a/gr-hamnet70/grc/hamnet70_freq_est_lr.xml b/gr-hamnet70/grc/hamnet70_freq_est_lr.xml new file mode 100644 index 0000000..339bd57 --- /dev/null +++ b/gr-hamnet70/grc/hamnet70_freq_est_lr.xml @@ -0,0 +1,37 @@ + + + L&R Data-Aided Freq. Est. + hamnet70_freq_est_lr + [hamnet70] + import hamnet70 + hamnet70.freq_est_lr($symbols, $kappa) + + + Preamble Symbols + symbols + complex_vector + + + + Kappa + kappa + int + + + + in + complex + + + + out + float + + + + freq_offset + message + 1 + + + diff --git a/gr-hamnet70/grc/hamnet70_pid_controller.xml b/gr-hamnet70/grc/hamnet70_pid_controller.xml new file mode 100644 index 0000000..4a78a72 --- /dev/null +++ b/gr-hamnet70/grc/hamnet70_pid_controller.xml @@ -0,0 +1,53 @@ + + + PID Controller + hamnet70_pid_controller + [hamnet70] + import hamnet70 + hamnet70.pid_controller($interval, $p, $i, $d, $post_gain) + + + interval + interval + 1 + int + + + + P Gain + p + 1.0 + float + + + + I Gain + i + 0.0 + float + + + + D Gain + d + 0.0 + float + + + + Post Gain + post_gain + 1.0 + float + + + + in + float + + + + control_value + message + + diff --git a/gr-hamnet70/include/hamnet70/CMakeLists.txt b/gr-hamnet70/include/hamnet70/CMakeLists.txt index 6008c74..ff38db1 100644 --- a/gr-hamnet70/include/hamnet70/CMakeLists.txt +++ b/gr-hamnet70/include/hamnet70/CMakeLists.txt @@ -23,5 +23,8 @@ ######################################################################## install(FILES api.h - correct_phase_from_tag.h DESTINATION include/hamnet70 + correct_phase_from_tag.h + correct_frequency.h + freq_est_lr.h + pid_controller.h DESTINATION include/hamnet70 ) diff --git a/gr-hamnet70/include/hamnet70/correct_frequency.h b/gr-hamnet70/include/hamnet70/correct_frequency.h new file mode 100644 index 0000000..acd6a3c --- /dev/null +++ b/gr-hamnet70/include/hamnet70/correct_frequency.h @@ -0,0 +1,56 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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_HAMNET70_CORRECT_FREQUENCY_H +#define INCLUDED_HAMNET70_CORRECT_FREQUENCY_H + +#include +#include + +namespace gr { + namespace hamnet70 { + + /*! + * \brief <+description of block+> + * \ingroup hamnet70 + * + */ + class HAMNET70_API correct_frequency : virtual public gr::sync_block + { + public: + typedef boost::shared_ptr sptr; + + /*! + * \brief Return a shared_ptr to a new instance of hamnet70::correct_frequency. + * + * To avoid accidental use of raw pointers, hamnet70::correct_frequency's + * constructor is in a private implementation + * class. hamnet70::correct_frequency::make is the public interface for + * creating new instances. + */ + static sptr make(const std::vector &symbols); + }; + + } // namespace hamnet70 +} // namespace gr + +#endif /* INCLUDED_HAMNET70_CORRECT_FREQUENCY_H */ + diff --git a/gr-hamnet70/include/hamnet70/freq_est_lr.h b/gr-hamnet70/include/hamnet70/freq_est_lr.h new file mode 100644 index 0000000..1aaaa16 --- /dev/null +++ b/gr-hamnet70/include/hamnet70/freq_est_lr.h @@ -0,0 +1,56 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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_HAMNET70_FREQ_EST_LR_H +#define INCLUDED_HAMNET70_FREQ_EST_LR_H + +#include +#include + +namespace gr { + namespace hamnet70 { + + /*! + * \brief <+description of block+> + * \ingroup hamnet70 + * + */ + class HAMNET70_API freq_est_lr : virtual public gr::block + { + public: + typedef boost::shared_ptr sptr; + + /*! + * \brief Return a shared_ptr to a new instance of hamnet70::freq_est_lr. + * + * To avoid accidental use of raw pointers, hamnet70::freq_est_lr's + * constructor is in a private implementation + * class. hamnet70::freq_est_lr::make is the public interface for + * creating new instances. + */ + static sptr make(const std::vector &symbols, size_t kappa = 3); + }; + + } // namespace hamnet70 +} // namespace gr + +#endif /* INCLUDED_HAMNET70_FREQ_EST_LR_H */ + diff --git a/gr-hamnet70/include/hamnet70/pid_controller.h b/gr-hamnet70/include/hamnet70/pid_controller.h new file mode 100644 index 0000000..474dead --- /dev/null +++ b/gr-hamnet70/include/hamnet70/pid_controller.h @@ -0,0 +1,56 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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_HAMNET70_PID_CONTROLLER_H +#define INCLUDED_HAMNET70_PID_CONTROLLER_H + +#include +#include + +namespace gr { + namespace hamnet70 { + + /*! + * \brief <+description of block+> + * \ingroup hamnet70 + * + */ + class HAMNET70_API pid_controller : virtual public gr::sync_block + { + public: + typedef boost::shared_ptr sptr; + + /*! + * \brief Return a shared_ptr to a new instance of hamnet70::pid_controller. + * + * To avoid accidental use of raw pointers, hamnet70::pid_controller's + * constructor is in a private implementation + * class. hamnet70::pid_controller::make is the public interface for + * creating new instances. + */ + static sptr make(size_t interval, float p, float i, float d, float post_gain = 1.0f); + }; + + } // namespace hamnet70 +} // namespace gr + +#endif /* INCLUDED_HAMNET70_PID_CONTROLLER_H */ + diff --git a/gr-hamnet70/lib/CMakeLists.txt b/gr-hamnet70/lib/CMakeLists.txt index d050af0..e558405 100644 --- a/gr-hamnet70/lib/CMakeLists.txt +++ b/gr-hamnet70/lib/CMakeLists.txt @@ -25,9 +25,11 @@ include(GrPlatform) #define LIB_SUFFIX include_directories(${Boost_INCLUDE_DIR}) link_directories(${Boost_LIBRARY_DIRS}) - list(APPEND hamnet70_sources correct_phase_from_tag_impl.cc + correct_frequency_impl.cc + freq_est_lr_impl.cc + pid_controller_impl.cc ) set(hamnet70_sources "${hamnet70_sources}" PARENT_SCOPE) diff --git a/gr-hamnet70/lib/correct_frequency_impl.cc b/gr-hamnet70/lib/correct_frequency_impl.cc new file mode 100644 index 0000000..bcd4891 --- /dev/null +++ b/gr-hamnet70/lib/correct_frequency_impl.cc @@ -0,0 +1,134 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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 +#include +#include +#include "correct_frequency_impl.h" + +namespace gr { + namespace hamnet70 { + + correct_frequency::sptr + correct_frequency::make(const std::vector &symbols) + { + return gnuradio::get_initial_sptr + (new correct_frequency_impl(symbols)); + } + + /* + * The private constructor + */ + correct_frequency_impl::correct_frequency_impl(const std::vector &symbols) + : gr::sync_block("correct_frequency", + gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(gr_complex))), + d_symbols(symbols), + d_correctionIncrement(1, 0), + d_correctionVector(1, 0), + d_filteredPhaseInc(0.0f), + d_preIdx(symbols.size()+1) // somewhere out of symbols' index range + {} + + /* + * Our virtual destructor. + */ + correct_frequency_impl::~correct_frequency_impl() + { + } + + int + correct_frequency_impl::work(int noutput_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]; + + std::vector tags; + get_tags_in_window(tags, 0, 0, noutput_items, pmt::intern("corr_est")); // FIXME: make name variable + size_t tagidx = 0; + + for(int i = 0; i < noutput_items; i++) { + if((tagidx < tags.size()) && (tags[tagidx].offset == nitems_read(0) + i)) { + d_preIdx = 0; + + tagidx++; + } + + if(d_preIdx < d_symbols.size()) { + gr_complex tmp = conj(d_symbols[d_preIdx]) * (*in); + float phase = gr::fast_atan2f(tmp); + + if(d_preIdx != 0) { + float phaseIncrement = phase - d_lastPhase; + + if(phaseIncrement > M_PI) { + add_item_tag(0, nitems_read(0) + i, pmt::intern("phase_overflow"), pmt::from_double(phaseIncrement)); + phaseIncrement -= 2*M_PI; + } else if(phaseIncrement < -M_PI) { + add_item_tag(0, nitems_read(0) + i, pmt::intern("phase_underflow"), pmt::from_double(phaseIncrement)); + phaseIncrement += 2*M_PI; + } + + /* + d_filteredPhaseInc = c_phaseInc_alpha * phaseIncrement + + (1 - c_phaseInc_alpha) * d_filteredPhaseInc; + */ + + d_filteredPhaseInc += phaseIncrement; + + if(d_preIdx > d_symbols.size()/2) { + d_correctionIncrement = gr_expj(-d_filteredPhaseInc/d_preIdx); + } + } else { + d_unwrapOffset = 0; + d_filteredPhaseInc = 0; + } + + d_lastPhase = phase; + + if(d_preIdx == d_symbols.size()/2) { + d_correctionVector = gr_complex(1, 0); + } + + d_preIdx++; + } + + d_correctionVector *= d_correctionIncrement; + + *out = *in * d_correctionVector; + + + in++; + out++; + } + + // Tell runtime system how many output items we produced. + return noutput_items; + } + + } /* namespace hamnet70 */ +} /* namespace gr */ + diff --git a/gr-hamnet70/lib/correct_frequency_impl.h b/gr-hamnet70/lib/correct_frequency_impl.h new file mode 100644 index 0000000..263cfce --- /dev/null +++ b/gr-hamnet70/lib/correct_frequency_impl.h @@ -0,0 +1,57 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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_HAMNET70_CORRECT_FREQUENCY_IMPL_H +#define INCLUDED_HAMNET70_CORRECT_FREQUENCY_IMPL_H + +#include + +namespace gr { + namespace hamnet70 { + + class correct_frequency_impl : public correct_frequency + { + private: + std::vector d_symbols; + gr_complex d_correctionVector; + gr_complex d_correctionIncrement; + + float d_lastPhase; + float d_filteredPhaseInc; + float d_unwrapOffset; + size_t d_preIdx; + + static const constexpr float c_phaseInc_alpha = 0.001f; + + public: + correct_frequency_impl(const std::vector &symbols); + ~correct_frequency_impl(); + + // Where all the action really happens + int work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + }; + + } // namespace hamnet70 +} // namespace gr + +#endif /* INCLUDED_HAMNET70_CORRECT_FREQUENCY_IMPL_H */ + diff --git a/gr-hamnet70/lib/correct_phase_from_tag_impl.cc b/gr-hamnet70/lib/correct_phase_from_tag_impl.cc index 02918ba..b38e270 100644 --- a/gr-hamnet70/lib/correct_phase_from_tag_impl.cc +++ b/gr-hamnet70/lib/correct_phase_from_tag_impl.cc @@ -65,8 +65,6 @@ namespace gr { std::vector tags; get_tags_in_window(tags, 0, 0, noutput_items, m_pmtPhaseTagName); - std::cerr << "correct_phase_from_tag: Found " << tags.size() << " tags." << std::endl; - size_t tagidx = 0; // rotate the data by the last set phase angle @@ -75,8 +73,6 @@ namespace gr { double phase = pmt::to_double(tags[tagidx].value); m_rotation = gr_expj(-phase); - std::cerr << "correct_phase_from_tag: Updated phase correction to " << -phase << std::endl; - tagidx++; } diff --git a/gr-hamnet70/lib/freq_est_lr_impl.cc b/gr-hamnet70/lib/freq_est_lr_impl.cc new file mode 100644 index 0000000..50f5345 --- /dev/null +++ b/gr-hamnet70/lib/freq_est_lr_impl.cc @@ -0,0 +1,141 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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 +#include +#include "freq_est_lr_impl.h" + +namespace gr { + namespace hamnet70 { + + freq_est_lr::sptr + freq_est_lr::make(const std::vector &symbols, size_t kappa) + { + return gnuradio::get_initial_sptr + (new freq_est_lr_impl(symbols, kappa)); + } + + /* + * The private constructor + */ + freq_est_lr_impl::freq_est_lr_impl(const std::vector &symbols, size_t kappa) + : gr::block("freq_est_lr", + gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(float))), + d_refSymbols(symbols), + d_kappa(kappa) + { + d_recvSymbols.reserve(symbols.size() + 1); + + message_port_register_out(pmt::mp("freq_offset")); + } + + /* + * Our virtual destructor. + */ + freq_est_lr_impl::~freq_est_lr_impl() + { + } + + void + freq_est_lr_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required) + { + ninput_items_required[0] = noutput_items; + } + + int + freq_est_lr_impl::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]; + float *out = (float *) output_items[0]; + + std::vector tags; + get_tags_in_window(tags, 0, 0, ninput_items[0], pmt::intern("corr_est")); // FIXME: make name variable + size_t tagidx = 0; + + for(size_t i = 0; i < noutput_items; i++) { + if((tagidx < tags.size()) && (tags[tagidx].offset == nitems_read(0) + i)) { + d_recvSymbols.clear(); + + tagidx++; + } + + if(d_recvSymbols.size() < d_refSymbols.size()) { + // buffer symbols of the preamble + d_recvSymbols.push_back(in[i]); + } else if(d_recvSymbols.size() == d_refSymbols.size()) { + // received enough symbols for the preamble => do frequency estimation + d_recvSymbols.push_back(0); //??? + + // remove reference symbols from the received preamble + std::vector z; + z.reserve(d_recvSymbols.size()); + for(size_t k = 0; k < d_recvSymbols.size(); k++) { + z.push_back( d_recvSymbols[k] * conj(d_refSymbols[k]) ); + } + + // Calculate averaged phase increments for sub-sequences + size_t N = d_recvSymbols.size()/2; + std::vector R_kappa(N); + + for(size_t kappa = 0; kappa < N; kappa++) { + for(size_t k = 0; k < d_recvSymbols.size() - kappa; k++) { + R_kappa[kappa] += z[k + kappa] * conj(z[k]); + } + + R_kappa[kappa] /= z.size() - kappa; + } + + // Calculate phase estimate (in radians/sample) + gr_complex sum_R_kappa(0, 0); + + for(size_t kappa = 0; kappa < N; kappa++) { + sum_R_kappa += R_kappa[kappa]; + } + + float arg = gr::fast_atan2f(sum_R_kappa); + + d_freq_est = arg / (M_PI * (1 + N)); + + message_port_pub(pmt::intern("freq_offset"), pmt::from_double(-d_freq_est)); + } + + out[i] = d_freq_est; + } + + // Do <+signal processing+> + // Tell runtime system how many input items we consumed on + // each input stream. + consume_each (noutput_items); + + // Tell runtime system how many output items we produced. + return noutput_items; + } + + } /* namespace hamnet70 */ +} /* namespace gr */ + diff --git a/gr-hamnet70/lib/freq_est_lr_impl.h b/gr-hamnet70/lib/freq_est_lr_impl.h new file mode 100644 index 0000000..5b6b812 --- /dev/null +++ b/gr-hamnet70/lib/freq_est_lr_impl.h @@ -0,0 +1,54 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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_HAMNET70_FREQ_EST_LR_IMPL_H +#define INCLUDED_HAMNET70_FREQ_EST_LR_IMPL_H + +#include + +namespace gr { + namespace hamnet70 { + + class freq_est_lr_impl : public freq_est_lr + { + private: + std::vector d_refSymbols; + std::vector d_recvSymbols; + size_t d_kappa; + float d_freq_est; + + public: + freq_est_lr_impl(const std::vector &symbols, size_t kappa); + ~freq_est_lr_impl(); + + // Where all the action really happens + 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); + }; + + } // namespace hamnet70 +} // namespace gr + +#endif /* INCLUDED_HAMNET70_FREQ_EST_LR_IMPL_H */ + diff --git a/gr-hamnet70/lib/pid_controller_impl.cc b/gr-hamnet70/lib/pid_controller_impl.cc new file mode 100644 index 0000000..558fd55 --- /dev/null +++ b/gr-hamnet70/lib/pid_controller_impl.cc @@ -0,0 +1,99 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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 +#include "pid_controller_impl.h" + +namespace gr { + namespace hamnet70 { + + pid_controller::sptr + pid_controller::make(size_t interval, float p, float i, float d, float post_gain) + { + return gnuradio::get_initial_sptr + (new pid_controller_impl(interval, p, i, d, post_gain)); + } + + /* + * The private constructor + */ + pid_controller_impl::pid_controller_impl(size_t interval, float p, float i, float d, float post_gain) + : gr::sync_block("pid_controller", + gr::io_signature::make(1, 1, sizeof(float)), + gr::io_signature::make(0, 0, 0)), + d_interval(interval), + d_lastInput(0), + d_iVal(0), + d_postGain(post_gain) + { + // TODO: scale with interval? + d_kp = p; + d_ki = i; + d_kd = d; + + message_port_register_out(pmt::mp("control_value")); + } + + /* + * Our virtual destructor. + */ + pid_controller_impl::~pid_controller_impl() + { + } + + int + pid_controller_impl::work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) + { + const float *in = (const float *) input_items[0]; + + while(d_nextSampleIdx < noutput_items) { + const float &inp = in[d_nextSampleIdx]; + + // proportional part + float control_value = d_kp * inp; + + // integral part + d_iVal += d_ki * inp; + control_value += d_iVal; + + // differential part + control_value += d_kd * (inp - d_lastInput); + d_lastInput = inp; + + message_port_pub(pmt::intern("control_value"), pmt::from_double(d_postGain * control_value)); + + d_nextSampleIdx += d_interval; + } + + d_nextSampleIdx -= noutput_items; + + // Tell runtime system how many output items we produced. + return noutput_items; + } + + } /* namespace hamnet70 */ +} /* namespace gr */ + diff --git a/gr-hamnet70/lib/pid_controller_impl.h b/gr-hamnet70/lib/pid_controller_impl.h new file mode 100644 index 0000000..587f41a --- /dev/null +++ b/gr-hamnet70/lib/pid_controller_impl.h @@ -0,0 +1,58 @@ +/* -*- c++ -*- */ +/* + * Copyright 2019 Thomas Kolb. + * + * 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_HAMNET70_PID_CONTROLLER_IMPL_H +#define INCLUDED_HAMNET70_PID_CONTROLLER_IMPL_H + +#include + +namespace gr { + namespace hamnet70 { + + class pid_controller_impl : public pid_controller + { + private: + float d_kp; + float d_ki; + float d_kd; + + float d_lastInput; + float d_iVal; + + float d_postGain; + + size_t d_interval; + size_t d_nextSampleIdx; + + public: + pid_controller_impl(size_t interval, float p, float i, float d, float post_gain); + ~pid_controller_impl(); + + // Where all the action really happens + int work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + }; + + } // namespace hamnet70 +} // namespace gr + +#endif /* INCLUDED_HAMNET70_PID_CONTROLLER_IMPL_H */ + diff --git a/gr-hamnet70/make.sh b/gr-hamnet70/make.sh index d42daec..164f99d 100755 --- a/gr-hamnet70/make.sh +++ b/gr-hamnet70/make.sh @@ -1,6 +1,12 @@ #!/bin/sh +set -e + mkdir -p build cd build cmake .. make -j4 + +if [ "$1" == "install" ]; then + sudo make install +fi diff --git a/gr-hamnet70/python/CMakeLists.txt b/gr-hamnet70/python/CMakeLists.txt index cb0f0f8..1a9a074 100644 --- a/gr-hamnet70/python/CMakeLists.txt +++ b/gr-hamnet70/python/CMakeLists.txt @@ -43,3 +43,6 @@ include(GrTest) set(GR_TEST_TARGET_DEPS gnuradio-hamnet70) set(GR_TEST_PYTHON_DIRS ${CMAKE_BINARY_DIR}/swig) #GR_ADD_TEST(qa_correct_phase_from_tag ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/qa_correct_phase_from_tag.py) +##GR_ADD_TEST(qa_correct_frequency ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/qa_correct_frequency.py) +#GR_ADD_TEST(qa_freq_est_lr ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/qa_freq_est_lr.py) +#GR_ADD_TEST(qa_pid_controller ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/qa_pid_controller.py) diff --git a/gr-hamnet70/python/qa_correct_frequency.py b/gr-hamnet70/python/qa_correct_frequency.py new file mode 100755 index 0000000..8dc6dfe --- /dev/null +++ b/gr-hamnet70/python/qa_correct_frequency.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# Copyright 2019 Thomas Kolb. +# +# 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. +# + +from gnuradio import gr, gr_unittest +from gnuradio import blocks +import hamnet70_swig as hamnet70 + +class qa_correct_frequency (gr_unittest.TestCase): + + def setUp (self): + self.tb = gr.top_block () + + def tearDown (self): + self.tb = None + + def test_001_t (self): + # set up fg + self.tb.run () + # check data + + +if __name__ == '__main__': + gr_unittest.run(qa_correct_frequency, "qa_correct_frequency.xml") diff --git a/gr-hamnet70/python/qa_freq_est_lr.py b/gr-hamnet70/python/qa_freq_est_lr.py new file mode 100755 index 0000000..64d8aa6 --- /dev/null +++ b/gr-hamnet70/python/qa_freq_est_lr.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# Copyright 2019 Thomas Kolb. +# +# 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. +# + +from gnuradio import gr, gr_unittest +from gnuradio import blocks +import hamnet70_swig as hamnet70 + +class qa_freq_est_lr (gr_unittest.TestCase): + + def setUp (self): + self.tb = gr.top_block () + + def tearDown (self): + self.tb = None + + def test_001_t (self): + # set up fg + self.tb.run () + # check data + + +if __name__ == '__main__': + gr_unittest.run(qa_freq_est_lr, "qa_freq_est_lr.xml") diff --git a/gr-hamnet70/python/qa_pid_controller.py b/gr-hamnet70/python/qa_pid_controller.py new file mode 100755 index 0000000..19a008e --- /dev/null +++ b/gr-hamnet70/python/qa_pid_controller.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# Copyright 2019 Thomas Kolb. +# +# 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. +# + +from gnuradio import gr, gr_unittest +from gnuradio import blocks +import hamnet70_swig as hamnet70 + +class qa_pid_controller (gr_unittest.TestCase): + + def setUp (self): + self.tb = gr.top_block () + + def tearDown (self): + self.tb = None + + def test_001_t (self): + # set up fg + self.tb.run () + # check data + + +if __name__ == '__main__': + gr_unittest.run(qa_pid_controller, "qa_pid_controller.xml") diff --git a/gr-hamnet70/swig/hamnet70_swig.i b/gr-hamnet70/swig/hamnet70_swig.i index e9317a8..ac1eef6 100644 --- a/gr-hamnet70/swig/hamnet70_swig.i +++ b/gr-hamnet70/swig/hamnet70_swig.i @@ -9,8 +9,18 @@ %{ #include "hamnet70/correct_phase_from_tag.h" +#include "hamnet70/correct_frequency.h" +#include "hamnet70/freq_est_lr.h" +#include "hamnet70/pid_controller.h" %} %include "hamnet70/correct_phase_from_tag.h" GR_SWIG_BLOCK_MAGIC2(hamnet70, correct_phase_from_tag); + +%include "hamnet70/correct_frequency.h" +GR_SWIG_BLOCK_MAGIC2(hamnet70, correct_frequency); +%include "hamnet70/freq_est_lr.h" +GR_SWIG_BLOCK_MAGIC2(hamnet70, freq_est_lr); +%include "hamnet70/pid_controller.h" +GR_SWIG_BLOCK_MAGIC2(hamnet70, pid_controller);