From d29958d51f2d99538973d39f6ec789d044ec366d Mon Sep 17 00:00:00 2001 From: f4exb Date: Thu, 17 May 2018 01:05:48 +0200 Subject: [PATCH] Added a frequency lock loop --- sdrbase/CMakeLists.txt | 4 ++ sdrbase/dsp/freqlockcomplex.cpp | 122 ++++++++++++++++++++++++++++++++ sdrbase/dsp/freqlockcomplex.h | 62 ++++++++++++++++ 3 files changed, 188 insertions(+) create mode 100644 sdrbase/dsp/freqlockcomplex.cpp create mode 100644 sdrbase/dsp/freqlockcomplex.h diff --git a/sdrbase/CMakeLists.txt b/sdrbase/CMakeLists.txt index 0459c898c..ba63ce3e4 100644 --- a/sdrbase/CMakeLists.txt +++ b/sdrbase/CMakeLists.txt @@ -34,6 +34,7 @@ set(sdrbase_SOURCES dsp/filterrc.cpp dsp/filtermbe.cpp dsp/filerecord.cpp + dsp/freqlockcomplex.cpp dsp/interpolator.cpp dsp/hbfiltertraits.cpp dsp/lowpass.cpp @@ -124,6 +125,7 @@ set(sdrbase_HEADERS dsp/filterrc.h dsp/filtermbe.h dsp/filerecord.h + dsp/freqlockcomplex.h dsp/gfft.h dsp/iirfilter.h dsp/interpolator.h @@ -271,6 +273,7 @@ include_directories( . ${CMAKE_SOURCE_DIR}/exports ${CMAKE_SOURCE_DIR}/httpserver + ${CMAKE_SOURCE_DIR}/kitiirfir ${CMAKE_SOURCE_DIR}/qrtplib ${CMAKE_SOURCE_DIR}/swagger/sdrangel/code/qt5/client ) @@ -278,6 +281,7 @@ include_directories( target_link_libraries(sdrbase ${QT_LIBRARIES} httpserver + kitiirfir qrtplib swagger ) diff --git a/sdrbase/dsp/freqlockcomplex.cpp b/sdrbase/dsp/freqlockcomplex.cpp new file mode 100644 index 000000000..d0d4a5772 --- /dev/null +++ b/sdrbase/dsp/freqlockcomplex.cpp @@ -0,0 +1,122 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018 F4EXB // +// written by Edouard Griffiths // +// // +// See: http://liquidsdr.org/blog/pll-howto/ // +// Fixed filter registers saturation // +// Added order for PSK locking. This brilliant idea actually comes from this // +// post: https://www.dsprelated.com/showthread/comp.dsp/36356-1.php // +// // +// This program 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 as version 3 of the License, or // +// // +// This program 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 V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#include "freqlockcomplex.h" +#include "IIRFilterCode.h" + +FreqLockComplex::FreqLockComplex() : + m_a0(1.0), + m_a1(1.0), + m_a2(1.0), + m_b0(1.0), + m_b1(1.0), + m_b2(1.0), + m_v0(0.0), + m_v1(0.0), + m_v2(0.0), + m_y(1.0, 0.0), + m_prod(1.0, 0.0), + m_yRe(1.0), + m_yIm(0.0), + m_freq(0.0), + m_phi(0.0), + m_iir(0) +{ +} + +FreqLockComplex::~FreqLockComplex() +{ + if (m_iir) { + delete m_iir; + } +} + +void FreqLockComplex::reset() +{ + m_v0 = 0.0f; + m_v1 = 0.0f; + m_v2 = 0.0f; + m_y.real(1.0); + m_y.imag(0.0); + m_prod.real(1.0); + m_prod.imag(0.0); + m_yRe = 1.0f; + m_yIm = 0.0f; + m_freq = 0.0f; + m_phi = 0.0f; +} + +// wn is in terms of Nyquist. For example, if the sampling frequency = 20 kHz +// and the 3 dB corner frequency is 1.5 kHz, then OmegaC = 0.15 +// i.e. 100.0 / (SR/2) or 200 / SR for 100 Hz +void FreqLockComplex::computeCoefficients(float wn) +{ + kitiirfir::TIIRFilterParams params; + + params.BW = 0.0; // For band pass and notch filters - unused here + params.Gamma = 0.0; // For Adjustable Gauss. - unused here + params.IIRPassType = kitiirfir::iirLPF; + params.NumPoles = 1; + params.OmegaC = wn; + params.ProtoType = kitiirfir::BUTTERWORTH; + params.Ripple = 0.0; // For Elliptic and Chebyshev - unused here + params.StopBanddB = 0.0; // For Elliptic and Inverse Chebyshev - unused here + params.dBGain = 0.0; + + kitiirfir::TIIRCoeff coeff = kitiirfir::CalcIIRFilterCoeff(params); + float a[3], b[3]; + + a[0] = coeff.a0[0]; + a[1] = coeff.a1[0]; + a[2] = coeff.a2[0]; + b[0] = coeff.b0[0]; + b[1] = coeff.b1[0]; + b[2] = coeff.b2[0]; + + qDebug("FreqLockComplex::computeCoefficients: b: %f %f %f", b[0], b[1], b[2]); + qDebug("FreqLockComplex::computeCoefficients: a: %f %f %f", a[0], a[1], a[2]); + + m_iir = new IIRFilter(a, b); +} + +void FreqLockComplex::feed(float re, float im) +{ + m_yRe = cos(m_phi); + m_yIm = sin(m_phi); + std::complex y(m_yRe, m_yIm); + std::complex x(re, im); + + std::complex prod = x * m_y; + + // Discriminator: cross * sign(dot) / dt + float cross = m_prod.real()*prod.imag() - prod.real()*m_prod.imag(); + float dot = m_prod.real()*prod.real() + m_prod.imag()*prod.imag(); + float eF = cross * (dot < 0 ? -1 : 1); // frequency error + + // LPF section + float efHat = m_iir->run(eF); + + m_freq = efHat; // correct instantaneous frequency + m_phi += efHat; // advance phase with instantaneous frequency + m_prod = prod; // store previous product +} + diff --git a/sdrbase/dsp/freqlockcomplex.h b/sdrbase/dsp/freqlockcomplex.h new file mode 100644 index 000000000..e860821d0 --- /dev/null +++ b/sdrbase/dsp/freqlockcomplex.h @@ -0,0 +1,62 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018 F4EXB // +// written by Edouard Griffiths // +// // +// See: http://liquidsdr.org/blog/pll-howto/ // +// Fixed filter registers saturation // +// Added order for PSK locking. This brilliant idea actually comes from this // +// post: https://www.dsprelated.com/showthread/comp.dsp/36356-1.php // +// // +// This program 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 as version 3 of the License, or // +// // +// This program 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 V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef SDRBASE_DSP_FREQLOCKCOMPLEX_H_ +#define SDRBASE_DSP_FREQLOCKCOMPLEX_H_ + +#include "dsp/dsptypes.h" +#include "iirfilter.h" +#include "export.h" + +/** General purpose Phase-locked loop using complex analytic signal input. */ +class SDRBASE_API FreqLockComplex +{ +public: + FreqLockComplex(); + ~FreqLockComplex(); + + void reset(); + void computeCoefficients(float wn); + /** Feed PLL with a new signa sample */ + void feed(float re, float im); + +private: + float m_a0; + float m_a1; + float m_a2; + float m_b0; + float m_b1; + float m_b2; + float m_v0; + float m_v1; + float m_v2; + std::complex m_y; + std::complex m_prod; + float m_yRe; + float m_yIm; + float m_freq; + float m_phi; + IIRFilter *m_iir; +}; + + +#endif /* SDRBASE_DSP_FREQLOCKCOMPLEX_H_ */