SDRAngel  4.11.5
Developer docs for <a href="https://github.com/f4exb/sdrangel">SDRangel<\a>, an Open Source Qt5 / OpenGL 3.0+ SDR and signal analyzer frontend to various hardware.
freqlockcomplex.cpp
Go to the documentation of this file.
1 // Copyright (C) 2018 F4EXB //
3 // written by Edouard Griffiths //
4 // //
5 // See: http://liquidsdr.org/blog/pll-howto/ //
6 // Fixed filter registers saturation //
7 // Added order for PSK locking. This brilliant idea actually comes from this //
8 // post: https://www.dsprelated.com/showthread/comp.dsp/36356-1.php //
9 // //
10 // This program is free software; you can redistribute it and/or modify //
11 // it under the terms of the GNU General Public License as published by //
12 // the Free Software Foundation as version 3 of the License, or //
13 // (at your option) any later version. //
14 // //
15 // This program is distributed in the hope that it will be useful, //
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of //
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
18 // GNU General Public License V3 for more details. //
19 // //
20 // You should have received a copy of the GNU General Public License //
21 // along with this program. If not, see <http://www.gnu.org/licenses/>. //
23 
24 #include "freqlockcomplex.h"
25 #define _USE_MATH_DEFINES
26 #include <math.h>
27 
29  m_a0(0.998),
30  m_a1(0.002),
31  m_y(1.0, 0.0),
32  m_yRe(1.0),
33  m_yIm(0.0),
34  m_freq(0.0),
35  m_phi(0.0),
36  m_phiX0(0.0),
37  m_phiX1(0.0),
38  m_y1(0.0f)
39 {
40 }
41 
43 {
44 }
45 
47 {
48  m_y.real(1.0);
49  m_y.imag(0.0);
50  m_yRe = 1.0f;
51  m_yIm = 0.0f;
52  m_freq = 0.0f;
53  m_phi = 0.0f;
54  m_phiX0 = 0.0f;
55  m_phiX1 = 0.0f;
56  m_y1 = 0.0f;
57 }
58 
59 void FreqLockComplex::setSampleRate(unsigned int sampleRate)
60 {
61  m_a1 = 10.0f / sampleRate; // 1 - alpha
62  m_a0 = 1.0f - m_a1; // alpha
63  reset();
64 }
65 
66 void FreqLockComplex::feed(float re, float im)
67 {
68  m_yRe = cos(m_phi);
69  m_yIm = sin(m_phi);
70  m_y.real(m_yRe);
71  m_y.imag(m_yIm);
72  std::complex<float> x(re, im);
73  m_phiX0 = std::arg(x);
74 
75  float eF = normalizeAngle(m_phiX0 - m_phiX1);
76  float fHat = m_a1*eF + m_a0*m_y1;
77  m_y1 = fHat;
78 
79  m_freq = fHat; // correct instantaneous frequency
80  m_phi += fHat; // advance phase with instantaneous frequency
81  m_phiX1 = m_phiX0;
82 }
83 
85 {
86  while (angle <= -M_PI) {
87  angle += 2.0*M_PI;
88  }
89  while (angle > M_PI) {
90  angle -= 2.0*M_PI;
91  }
92  return angle;
93 }
94 
Fixed< IntType, IntBits > cos(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2271
std::complex< float > m_y
#define M_PI
Definition: rdsdemod.cpp:27
Fixed< IntType, IntBits > arg(const std::complex< Fixed< IntType, IntBits > > &val)
Definition: fixed.h:2401
static float normalizeAngle(float angle)
Fixed< IntType, IntBits > sin(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2265
void feed(float re, float im)
void setSampleRate(unsigned int sampleRate)