From 1549ecaa0faeeb4371904ba48552e51ceba9a153 Mon Sep 17 00:00:00 2001 From: f4exb Date: Sun, 13 May 2018 08:55:14 +0200 Subject: [PATCH] New PLL with complex signal input and w, zeta, K parameters --- sdrbase/CMakeLists.txt | 2 + sdrbase/dsp/phaselockcomplex.cpp | 137 +++++++++++++++++++++++++++++++ sdrbase/dsp/phaselockcomplex.h | 70 ++++++++++++++++ 3 files changed, 209 insertions(+) create mode 100644 sdrbase/dsp/phaselockcomplex.cpp create mode 100644 sdrbase/dsp/phaselockcomplex.h diff --git a/sdrbase/CMakeLists.txt b/sdrbase/CMakeLists.txt index 9a55e6dd8..0459c898c 100644 --- a/sdrbase/CMakeLists.txt +++ b/sdrbase/CMakeLists.txt @@ -40,6 +40,7 @@ set(sdrbase_SOURCES dsp/nco.cpp dsp/ncof.cpp dsp/phaselock.cpp + dsp/phaselockcomplex.cpp dsp/projector.cpp dsp/samplesinkfifo.cpp dsp/samplesourcefifo.cpp @@ -146,6 +147,7 @@ set(sdrbase_HEADERS dsp/ncof.h dsp/phasediscri.h dsp/phaselock.h + dsp/phaselockcomplex.h dsp/projector.h dsp/recursivefilters.h dsp/samplesinkfifo.h diff --git a/sdrbase/dsp/phaselockcomplex.cpp b/sdrbase/dsp/phaselockcomplex.cpp new file mode 100644 index 000000000..1fedb512d --- /dev/null +++ b/sdrbase/dsp/phaselockcomplex.cpp @@ -0,0 +1,137 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018 F4EXB // +// written by Edouard Griffiths // +// // +// See: http://liquidsdr.org/blog/pll-howto/ // +// Fixes filter registers saturation // +// // +// 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 +#include +#include "phaselockcomplex.h" + +PhaseLockComplex::PhaseLockComplex() : + 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_deltaPhi(0.0), + m_phiHatLast(0.0), + m_phiHat(0.0), + m_y(1.0, 0.0), + m_yRe(1.0), + m_yIm(0.0), + m_freq(0.0) +{ +} + +void PhaseLockComplex::computeCoefficients(Real wn, Real zeta, Real K) +{ + double t1 = K/(wn*wn); // + double t2 = 2*zeta/wn - 1/K; // + + double b0 = 2*K*(1.+t2/2.0f); + double b1 = 2*K*2.; + double b2 = 2*K*(1.-t2/2.0f); + + double a0 = 1 + t1/2.0f; + double a1 = -t1; + double a2 = -1 + t1/2.0f; + + qDebug("PhaseLockComplex::computeCoefficients: b_raw: %f %f %f", b0, b1, b2); + qDebug("PhaseLockComplex::computeCoefficients: a_raw: %f %f %f", a0, a1, a2); + + m_b0 = b0 / a0; + m_b1 = b1 / a0; + m_b2 = b2 / a0; + + // a0 = 1.0 is implied + m_a1 = a1 / a0; + m_a2 = a2 / a0; + + qDebug("PhaseLockComplex::computeCoefficients: b: %f %f %f", m_b0, m_b1, m_b2); + qDebug("PhaseLockComplex::computeCoefficients: a: 1.0 %f %f", m_a1, m_a2); + + reset(); +} + +void PhaseLockComplex::reset() +{ + // reset filter accumulators and phase + m_v0 = 0.0f; + m_v1 = 0.0f; + m_v2 = 0.0f; + m_deltaPhi = 0.0f; + m_phiHatLast = 0.0f; + m_phiHat = 0.0f; + m_y.real(1.0); + m_y.real(0.0); + m_yRe = 1.0f; + m_yIm = 0.0f; + m_freq = 0.0f; +} + +void PhaseLockComplex::feed(float re, float im) +{ + m_yRe = cos(m_phiHat); + m_yIm = sin(m_phiHat); + m_y.real(m_yRe); + m_y.imag(m_yIm); + std::complex x(re, im); + m_deltaPhi = std::arg(x * std::conj(m_y)); + + // advance buffer + m_v2 = m_v1; // shift center register to upper register + m_v1 = m_v0; // shift lower register to center register + + // compute new lower register + m_v0 = m_deltaPhi - m_v1*m_a1 - m_v2*m_a2; + + // compute new output + m_phiHat = m_v0*m_b0 + m_v1*m_b1 + m_v2*m_b2; + + // prevent saturation + if (m_phiHat > 2.0*M_PI) + { + m_v0 *= (m_phiHat - 2.0*M_PI) / m_phiHat; + m_v1 *= (m_phiHat - 2.0*M_PI) / m_phiHat; + m_v2 *= (m_phiHat - 2.0*M_PI) / m_phiHat; + m_phiHat -= 2.0*M_PI; + } + + if (m_phiHat < -2.0*M_PI) + { + m_v0 *= (m_phiHat + 2.0*M_PI) / m_phiHat; + m_v1 *= (m_phiHat + 2.0*M_PI) / m_phiHat; + m_v2 *= (m_phiHat + 2.0*M_PI) / m_phiHat; + m_phiHat += 2.0*M_PI; + } + + m_freq = (m_phiHat - m_phiHatLast) / (2.0*M_PI); + + if (m_freq < -1.0f) { + m_freq += 2.0f; + } else if (m_freq > 1.0f) { + m_freq -= 2.0f; + } + + m_phiHatLast = m_phiHat; +} + + diff --git a/sdrbase/dsp/phaselockcomplex.h b/sdrbase/dsp/phaselockcomplex.h new file mode 100644 index 000000000..de5479c51 --- /dev/null +++ b/sdrbase/dsp/phaselockcomplex.h @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018 F4EXB // +// written by Edouard Griffiths // +// // +// See: http://liquidsdr.org/blog/pll-howto/ // +// Fixes filter registers saturation // +// // +// 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_PHASELOCKCOMPLEX_H_ +#define SDRBASE_DSP_PHASELOCKCOMPLEX_H_ + +#include "dsp/dsptypes.h" +#include "export.h" + +/** General purpose Phase-locked loop using complex analytic signal input. */ +class SDRBASE_API PhaseLockComplex +{ +public: + PhaseLockComplex(); + /** Compute loop filter parameters (active PI design) + * \param wn PLL bandwidth relative to Nyquist frequency + * \param zeta PLL damping factor + * \param K PLL loop gain + * */ + void computeCoefficients(Real wn, Real zeta, Real K); + void reset(); + void feed(float re, float im); + const std::complex& getComplex() const { return m_y; } + float getReal() const { return m_yRe; } + float getImag() const { return m_yIm; } + bool locked() const { return (m_deltaPhi > -0.1) && (m_deltaPhi < 0.1); } + float getFrequency() const { return m_freq; } + float getDeltaPhi() const { return m_deltaPhi; } + float getPhiHat() const { return m_phiHat; } + +private: + // a0 = 1 is implied + float m_a1; + float m_a2; + float m_b0; + float m_b1; + float m_b2; + float m_v0; + float m_v1; + float m_v2; + float m_deltaPhi; + float m_phiHatLast; + float m_phiHat; + std::complex m_y; + float m_yRe; + float m_yIm; + float m_freq; + +}; + + + +#endif /* SDRBASE_DSP_PHASELOCKCOMPLEX_H_ */