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.
phaselockcomplex.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 <complex.h>
25 #define _USE_MATH_DEFINES
26 #include <math.h>
27 #include "phaselockcomplex.h"
28 
30  m_a1(1.0),
31  m_a2(1.0),
32  m_b0(1.0),
33  m_b1(1.0),
34  m_b2(1.0),
35  m_v0(0.0),
36  m_v1(0.0),
37  m_v2(0.0),
38  m_deltaPhi(0.0),
39  m_phiHat(0.0),
40  m_phiHatPrev(0.0),
41  m_y(1.0, 0.0),
42  m_p(1.0, 0.0),
43  m_yRe(1.0),
44  m_yIm(0.0),
45  m_freq(0.0),
46  m_freqPrev(0.0),
47  m_freqTest(0.0),
48  m_lockCount(0),
49  m_lockFreq(0.026f),
50  m_pskOrder(1),
51  m_lockTime(480),
52  m_lockTimeCount(0)
53 {
54 }
55 
57 {
58  double t1 = K/(wn*wn); //
59  double t2 = 2*zeta/wn - 1/K; //
60 
61  double b0 = 2*K*(1.+t2/2.0f);
62  double b1 = 2*K*2.;
63  double b2 = 2*K*(1.-t2/2.0f);
64 
65  double a0 = 1 + t1/2.0f;
66  double a1 = -t1;
67  double a2 = -1 + t1/2.0f;
68 
69  qDebug("PhaseLockComplex::computeCoefficients: b_raw: %f %f %f", b0, b1, b2);
70  qDebug("PhaseLockComplex::computeCoefficients: a_raw: %f %f %f", a0, a1, a2);
71 
72  m_b0 = b0 / a0;
73  m_b1 = b1 / a0;
74  m_b2 = b2 / a0;
75 
76  // a0 = 1.0 is implied
77  m_a1 = a1 / a0;
78  m_a2 = a2 / a0;
79 
80  qDebug("PhaseLockComplex::computeCoefficients: b: %f %f %f", m_b0, m_b1, m_b2);
81  qDebug("PhaseLockComplex::computeCoefficients: a: 1.0 %f %f", m_a1, m_a2);
82 
83  reset();
84 }
85 
86 void PhaseLockComplex::setPskOrder(unsigned int order)
87 {
88  m_pskOrder = order > 0 ? order : 1;
89  reset();
90 }
91 
92 void PhaseLockComplex::setSampleRate(unsigned int sampleRate)
93 {
94  m_lockTime = sampleRate / 100; // 10ms for order 1
95  m_lockFreq = (2.0*M_PI*(m_pskOrder > 1 ? 6.0 : 1.0)) / sampleRate; // +/- 6 Hz frequency swing
96  reset();
97 }
98 
100 {
101  // reset filter accumulators and phase
102  m_v0 = 0.0f;
103  m_v1 = 0.0f;
104  m_v2 = 0.0f;
105  m_deltaPhi = 0.0f;
106  m_phiHat = 0.0f;
107  m_phiHatPrev = 0.0f;
108  m_y.real(1.0);
109  m_y.imag(0.0);
110  m_p.real(1.0);
111  m_p.imag(0.0);
112  m_yRe = 1.0f;
113  m_yIm = 0.0f;
114  m_freq = 0.0f;
115  m_freqPrev = 0.0f;
116  m_freqTest = 0.0f;
117  m_lockCount = 0;
118  m_lockTimeCount = 0;
119 }
120 
121 void PhaseLockComplex::feed(float re, float im)
122 {
123  m_yRe = cos(m_phiHat);
124  m_yIm = sin(m_phiHat);
125  m_y.real(m_yRe);
126  m_y.imag(m_yIm);
127  std::complex<float> x(re, im);
128  m_deltaPhi = std::arg(x * std::conj(m_y));
129 
130  // bring phase 0 on any of the PSK symbols
131  if (m_pskOrder > 1) {
133  }
134 
135  // advance buffer
136  m_v2 = m_v1; // shift center register to upper register
137  m_v1 = m_v0; // shift lower register to center register
138 
139  // compute new lower register
141 
142  // compute new output
144 
145  // prevent saturation
146  if (m_phiHat > 2.0*M_PI)
147  {
148  m_v0 *= (m_phiHat - 2.0*M_PI) / m_phiHat;
149  m_v1 *= (m_phiHat - 2.0*M_PI) / m_phiHat;
150  m_v2 *= (m_phiHat - 2.0*M_PI) / m_phiHat;
151  m_phiHat -= 2.0*M_PI;
152  }
153 
154  if (m_phiHat < -2.0*M_PI)
155  {
156  m_v0 *= (m_phiHat + 2.0*M_PI) / m_phiHat;
157  m_v1 *= (m_phiHat + 2.0*M_PI) / m_phiHat;
158  m_v2 *= (m_phiHat + 2.0*M_PI) / m_phiHat;
159  m_phiHat += 2.0*M_PI;
160  }
161 
162  // lock and frequency estimation
163  if (m_pskOrder > 1)
164  {
165  float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
166  m_freq = m_expAvg.feed(dPhi);
167 
168  if (m_lockTimeCount < m_lockTime-1)
169  {
170  m_lockTimeCount++;
171  }
172  else
173  {
174  float dF = m_freq - m_freqTest;
175 
176  if ((dF > -m_lockFreq) && (dF < m_lockFreq))
177  {
178  if (m_lockCount < 20) {
179  m_lockCount++;
180  }
181  }
182  else
183  {
184  if (m_lockCount > 0) {
185  m_lockCount--;
186  }
187  }
188 
189  m_freqTest = m_freq;
190  m_lockTimeCount = 0;
191  }
192 
194  }
195  else
196  {
199 
200  float dFreq = m_freqTest - m_freqPrev;
201 
202  if ((dFreq > -0.01) && (dFreq < 0.01))
203  {
204  if (m_lockCount < (m_lockTime-1)) { // [0..479]
205  m_lockCount++;
206  }
207  }
208  else
209  {
210  m_lockCount = 0;
211  }
212 
214  m_freqPrev = m_freqTest;
215  }
216 }
217 
219 {
220  while (angle <= -M_PI) {
221  angle += 2.0*M_PI;
222  }
223  while (angle > M_PI) {
224  angle -= 2.0*M_PI;
225  }
226  return angle;
227 }
Fixed< IntType, IntBits > cos(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2271
#define M_PI
Definition: rdsdemod.cpp:27
uint8_t b2
Definition: decimators.h:57
Fixed< IntType, IntBits > arg(const std::complex< Fixed< IntType, IntBits > > &val)
Definition: fixed.h:2401
std::complex< float > m_y
uint8_t b1
Definition: decimators.h:56
static float normalizeAngle(float angle)
Fixed< IntType, IntBits > sin(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2265
unsigned int m_pskOrder
std::complex< float > m_p
uint8_t b0
Definition: decimators.h:55
void computeCoefficients(Real wn, Real zeta, Real K)
float feed(const float &x)
void setPskOrder(unsigned int order)
float Real
Definition: dsptypes.h:42
void feed(float re, float im)
void setSampleRate(unsigned int sampleRate)