amateurfunk/hamnet70-gnuradio-legacy
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Added frequency estimator blocks

Browse source 
- 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.
This commit is contained in:
Thomas Kolb 2019-07-23 23:16:48 +02:00
parent 55cfbd7ab0
commit cd9784efb6
23 changed files with 980 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
gr-hamnet70/CMakeLists.txt
View file

@ -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)

6
gr-hamnet70/grc/CMakeLists.txt
View file

@ -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
)

25
gr-hamnet70/grc/hamnet70_correct_frequency.xml Normal file
View file

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<block>
<name>Correct Frequency</name>
<key>hamnet70_correct_frequency</key>
<category>[hamnet70]</category>
<import>import hamnet70</import>
<make>hamnet70.correct_frequency($symbols)</make>
<param>
<name>Symbols</name>
<key>symbols</key>
<type>complex_vector</type>
</param>
<sink>
<name>in</name>
<type>complex</type>
</sink>
<source>
<name>out</name>
<type>complex</type>
</source>
</block>

37
gr-hamnet70/grc/hamnet70_freq_est_lr.xml Normal file
View file

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<block>
<name>L&amp;R Data-Aided Freq. Est.</name>
<key>hamnet70_freq_est_lr</key>
<category>[hamnet70]</category>
<import>import hamnet70</import>
<make>hamnet70.freq_est_lr($symbols, $kappa)</make>
<param>
<name>Preamble Symbols</name>
<key>symbols</key>
<type>complex_vector</type>
</param>
<param>
<name>Kappa</name>
<key>kappa</key>
<type>int</type>
</param>
<sink>
<name>in</name>
<type>complex</type>
</sink>
<source>
<name>out</name>
<type>float</type>
</source>
<source>
<name>freq_offset</name>
<type>message</type>
<optional>1</optional>
</source>
</block>

53
gr-hamnet70/grc/hamnet70_pid_controller.xml Normal file
View file

@ -0,0 +1,53 @@
<?xml version="1.0"?>
<block>
<name>PID Controller</name>
<key>hamnet70_pid_controller</key>
<category>[hamnet70]</category>
<import>import hamnet70</import>
<make>hamnet70.pid_controller($interval, $p, $i, $d, $post_gain)</make>
<param>
<name>interval</name>
<key>interval</key>
<value>1</value>
<type>int</type>
</param>
<param>
<name>P Gain</name>
<key>p</key>
<value>1.0</value>
<type>float</type>
</param>
<param>
<name>I Gain</name>
<key>i</key>
<value>0.0</value>
<type>float</type>
</param>
<param>
<name>D Gain</name>
<key>d</key>
<value>0.0</value>
<type>float</type>
</param>
<param>
<name>Post Gain</name>
<key>post_gain</key>
<value>1.0</value>
<type>float</type>
</param>
<sink>
<name>in</name>
<type>float</type>
</sink>
<source>
<name>control_value</name>
<type>message</type>
</source>
</block>

5
gr-hamnet70/include/hamnet70/CMakeLists.txt
View file

@ -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
)

56
gr-hamnet70/include/hamnet70/correct_frequency.h Normal file
View file

@ -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 <hamnet70/api.h>
#include <gnuradio/sync_block.h>
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<correct_frequency> 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<gr_complex> &symbols);
};
} // namespace hamnet70
} // namespace gr
#endif /* INCLUDED_HAMNET70_CORRECT_FREQUENCY_H */

56
gr-hamnet70/include/hamnet70/freq_est_lr.h Normal file
View file

@ -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 <hamnet70/api.h>
#include <gnuradio/block.h>
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<freq_est_lr> 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<gr_complex> &symbols, size_t kappa = 3);
};
} // namespace hamnet70
} // namespace gr
#endif /* INCLUDED_HAMNET70_FREQ_EST_LR_H */

56
gr-hamnet70/include/hamnet70/pid_controller.h Normal file
View file

@ -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 <hamnet70/api.h>
#include <gnuradio/sync_block.h>
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<pid_controller> 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 */

4
gr-hamnet70/lib/CMakeLists.txt
View file

@ -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)

134
gr-hamnet70/lib/correct_frequency_impl.cc Normal file
View file

@ -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 <gnuradio/io_signature.h>
#include <gnuradio/expj.h>
#include <gnuradio/math.h>
#include "correct_frequency_impl.h"
namespace gr {
namespace hamnet70 {
correct_frequency::sptr
correct_frequency::make(const std::vector<gr_complex> &symbols)
{
return gnuradio::get_initial_sptr
(new correct_frequency_impl(symbols));
}
/*
* The private constructor
*/
correct_frequency_impl::correct_frequency_impl(const std::vector<gr_complex> &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<tag_t> 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 */

57
gr-hamnet70/lib/correct_frequency_impl.h Normal file
View file

@ -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 <hamnet70/correct_frequency.h>
namespace gr {
namespace hamnet70 {
class correct_frequency_impl : public correct_frequency
{
private:
std::vector<gr_complex> 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<gr_complex> &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 */

4
gr-hamnet70/lib/correct_phase_from_tag_impl.cc
View file

@ -65,8 +65,6 @@ namespace gr {
std::vector<tag_t> 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++;
}

141
gr-hamnet70/lib/freq_est_lr_impl.cc Normal file
View file

@ -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 <gnuradio/io_signature.h>
#include <gnuradio/math.h>
#include "freq_est_lr_impl.h"
namespace gr {
namespace hamnet70 {
freq_est_lr::sptr
freq_est_lr::make(const std::vector<gr_complex> &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<gr_complex> &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<tag_t> 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<gr_complex> 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 <N> sub-sequences
size_t N = d_recvSymbols.size()/2;
std::vector<gr_complex> 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 */

54
gr-hamnet70/lib/freq_est_lr_impl.h Normal file
View file

@ -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 <hamnet70/freq_est_lr.h>
namespace gr {
namespace hamnet70 {
class freq_est_lr_impl : public freq_est_lr
{
private:
std::vector<gr_complex> d_refSymbols;
std::vector<gr_complex> d_recvSymbols;
size_t d_kappa;
float d_freq_est;
public:
freq_est_lr_impl(const std::vector<gr_complex> &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 */

99
gr-hamnet70/lib/pid_controller_impl.cc Normal file
View file

@ -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 <gnuradio/io_signature.h>
#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 */

58
gr-hamnet70/lib/pid_controller_impl.h Normal file
View file

@ -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 <hamnet70/pid_controller.h>
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 */

6
gr-hamnet70/make.sh
View file

@ -1,6 +1,12 @@
#!/bin/sh
set -e
mkdir -p build
cd build
cmake ..
make -j4
if [ "$1" == "install" ]; then
sudo make install
fi

3
gr-hamnet70/python/CMakeLists.txt
View file

@ -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)

41
gr-hamnet70/python/qa_correct_frequency.py Executable file
View file

@ -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")

41
gr-hamnet70/python/qa_freq_est_lr.py Executable file
View file

@ -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")

41
gr-hamnet70/python/qa_pid_controller.py Executable file
View file

@ -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")

10
gr-hamnet70/swig/hamnet70_swig.i
View file

@ -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);