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.
phaselock.cpp
Go to the documentation of this file.
1 // Copyright (C) 2015 F4EXB //
3 // written by Edouard Griffiths //
4 // //
5 // This program is free software; you can redistribute it and/or modify //
6 // it under the terms of the GNU General Public License as published by //
7 // the Free Software Foundation as version 3 of the License, or //
8 // (at your option) any later version. //
9 // //
10 // This program is distributed in the hope that it will be useful, //
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of //
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
13 // GNU General Public License V3 for more details. //
14 // //
15 // You should have received a copy of the GNU General Public License //
16 // along with this program. If not, see <http://www.gnu.org/licenses/>. //
18 
19 #include <math.h>
20 #include "dsp/phaselock.h"
21 
22 #undef M_PI
23 #define M_PI 3.14159265358979323846
24 
25 // Construct phase-locked loop.
26 PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
27 {
28  /*
29  * This is a type-2, 4th order phase-locked loop.
30  *
31  * Open-loop transfer function:
32  * G(z) = K * (z - q1) / ((z - p1) * (z - p2) * (z - 1) * (z - 1))
33  * K = 3.788 * (bandwidth * 2 * Pi)**3
34  * q1 = exp(-0.1153 * bandwidth * 2*Pi)
35  * p1 = exp(-1.146 * bandwidth * 2*Pi)
36  * p2 = exp(-5.331 * bandwidth * 2*Pi)
37  *
38  * I don't understand what I'm doing; hopefully it will work.
39  */
40 
41  // Set min/max locking frequencies.
42  m_minfreq = (freq - bandwidth) * 2.0 * M_PI;
43  m_maxfreq = (freq + bandwidth) * 2.0 * M_PI;
44 
45  // Set valid signal threshold.
46  m_minsignal = minsignal;
47  m_lock_delay = int(20.0 / bandwidth);
48  m_lock_cnt = 0;
49  m_pilot_level = 0;
50  m_psin = 0.0;
51  m_pcos = 1.0;
52 
53  // Create 2nd order filter for I/Q representation of phase error.
54  // Filter has two poles, unit DC gain.
55  double p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
56  double p2 = exp(-5.331 * bandwidth * 2.0 * M_PI);
57  m_phasor_a1 = - p1 - p2;
58  m_phasor_a2 = p1 * p2;
60 
61  // Create loop filter to stabilize the loop.
62  double q1 = exp(-0.1153 * bandwidth * 2.0 * M_PI);
63  m_loopfilter_b0 = 0.62 * bandwidth * 2.0 * M_PI;
65 
66  // After the loop filter, the phase error is integrated to produce
67  // the frequency. Then the frequency is integrated to produce the phase.
68  // These integrators form the two remaining poles, both at z = 1.
69 
70  // Initialize frequency and phase.
71  m_freq = freq * 2.0 * M_PI;
72  m_phase = 0;
73 
74  m_phasor_i1 = 0;
75  m_phasor_i2 = 0;
76  m_phasor_q1 = 0;
77  m_phasor_q2 = 0;
78  m_loopfilter_x1 = 0;
79 
80  // Initialize PPS generator.
81  m_pilot_periods = 0;
82  m_pps_cnt = 0;
83  m_sample_cnt = 0;
84 }
85 
86 
87 void PhaseLock::configure(Real freq, Real bandwidth, Real minsignal)
88 {
89  qDebug("PhaseLock::configure: freq: %f bandwidth: %f minsignal: %f", freq, bandwidth, minsignal);
90  /*
91  * This is a type-2, 4th order phase-locked loop.
92  *
93  * Open-loop transfer function:
94  * G(z) = K * (z - q1) / ((z - p1) * (z - p2) * (z - 1) * (z - 1))
95  * K = 3.788 * (bandwidth * 2 * Pi)**3
96  * q1 = exp(-0.1153 * bandwidth * 2*Pi)
97  * p1 = exp(-1.146 * bandwidth * 2*Pi)
98  * p2 = exp(-5.331 * bandwidth * 2*Pi)
99  *
100  * I don't understand what I'm doing; hopefully it will work.
101  */
102 
103  // Set min/max locking frequencies.
104  m_minfreq = (freq - bandwidth) * 2.0 * M_PI;
105  m_maxfreq = (freq + bandwidth) * 2.0 * M_PI;
106 
107  // Set valid signal threshold.
108  m_minsignal = minsignal;
109  m_lock_delay = int(20.0 / bandwidth);
110  m_lock_cnt = 0;
111  m_pilot_level = 0;
112 
113  // Create 2nd order filter for I/Q representation of phase error.
114  // Filter has two poles, unit DC gain.
115  double p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
116  double p2 = exp(-5.331 * bandwidth * 2.0 * M_PI);
117  m_phasor_a1 = - p1 - p2;
118  m_phasor_a2 = p1 * p2;
120 
121  // Create loop filter to stabilize the loop.
122  double q1 = exp(-0.1153 * bandwidth * 2.0 * M_PI);
123  m_loopfilter_b0 = 0.62 * bandwidth * 2.0 * M_PI;
125 
126  // After the loop filter, the phase error is integrated to produce
127  // the frequency. Then the frequency is integrated to produce the phase.
128  // These integrators form the two remaining poles, both at z = 1.
129 
130  // Initialize frequency and phase.
131  m_freq = freq * 2.0 * M_PI;
132  m_phase = 0;
133 
134  m_phasor_i1 = 0;
135  m_phasor_i2 = 0;
136  m_phasor_q1 = 0;
137  m_phasor_q2 = 0;
138  m_loopfilter_x1 = 0;
139 
140  // Initialize PPS generator.
141  m_pilot_periods = 0;
142  m_pps_cnt = 0;
143  m_sample_cnt = 0;
144 }
145 
146 
147 // Process samples. Bufferized version
148 void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>& samples_out)
149 {
150  unsigned int n = samples_in.size();
151 
152  samples_out.resize(n);
153 
154  bool was_locked = (m_lock_cnt >= m_lock_delay);
155  m_pps_events.clear();
156 
157  if (n > 0)
158  m_pilot_level = 1000.0;
159 
160  for (unsigned int i = 0; i < n; i++) {
161 
162  // Generate locked pilot tone.
163  Real psin = sin(m_phase);
164  Real pcos = cos(m_phase);
165 
166  // Generate double-frequency output.
167  // sin(2*x) = 2 * sin(x) * cos(x)
168  samples_out[i] = 2 * psin * pcos;
169 
170  // Multiply locked tone with input.
171  Real x = samples_in[i];
172  Real phasor_i = psin * x;
173  Real phasor_q = pcos * x;
174 
175  // Run IQ phase error through low-pass filter.
176  phasor_i = m_phasor_b0 * phasor_i
179  phasor_q = m_phasor_b0 * phasor_q
182  m_phasor_i2 = m_phasor_i1;
183  m_phasor_i1 = phasor_i;
184  m_phasor_q2 = m_phasor_q1;
185  m_phasor_q1 = phasor_q;
186 
187  // Convert I/Q ratio to estimate of phase error.
188  Real phase_err;
189  if (phasor_i > std::abs(phasor_q)) {
190  // We are within +/- 45 degrees from lock.
191  // Use simple linear approximation of arctan.
192  phase_err = phasor_q / phasor_i;
193  } else if (phasor_q > 0) {
194  // We are lagging more than 45 degrees behind the input.
195  phase_err = 1;
196  } else {
197  // We are more than 45 degrees ahead of the input.
198  phase_err = -1;
199  }
200 
201  // Detect pilot level (conservative).
202  m_pilot_level = std::min(m_pilot_level, phasor_i);
203 
204  // Run phase error through loop filter and update frequency estimate.
205  m_freq += m_loopfilter_b0 * phase_err
207  m_loopfilter_x1 = phase_err;
208 
209  // Limit frequency to allowable range.
211 
212  // Update locked phase.
213  m_phase += m_freq;
214  if (m_phase > 2.0 * M_PI) {
215  m_phase -= 2.0 * M_PI;
216  m_pilot_periods++;
217 
218  // Generate pulse-per-second.
220  m_pilot_periods = 0;
221  if (was_locked) {
222  struct PpsEvent ev;
223  ev.pps_index = m_pps_cnt;
224  ev.sample_index = m_sample_cnt + i;
225  ev.block_position = double(i) / double(n);
226  m_pps_events.push_back(ev);
227  m_pps_cnt++;
228  }
229  }
230  }
231  }
232 
233  // Update lock status.
234  if (2 * m_pilot_level > m_minsignal) {
235  if (m_lock_cnt < m_lock_delay)
236  m_lock_cnt += n;
237  } else {
238  m_lock_cnt = 0;
239  }
240 
241  // Drop PPS events when pilot not locked.
242  if (m_lock_cnt < m_lock_delay) {
243  m_pilot_periods = 0;
244  m_pps_cnt = 0;
245  m_pps_events.clear();
246  }
247 
248  // Update sample counter.
249  m_sample_cnt += n;
250 }
251 
252 
253 // Process samples. Multiple output
254 void PhaseLock::process(const Real& sample_in, Real *samples_out)
255 {
256  m_pps_events.clear();
257 
258  // Generate locked pilot tone.
259  m_psin = sin(m_phase);
260  m_pcos = cos(m_phase);
261 
262  // Generate output
263  processPhase(samples_out);
264 
265  // Multiply locked tone with input.
266  Real phasor_i = m_psin * sample_in;
267  Real phasor_q = m_pcos * sample_in;
268 
269  // Actual PLL
270  process_phasor(phasor_i, phasor_q);
271 }
272 
273 void PhaseLock::process(const Real& real_in, const Real& imag_in, Real *samples_out)
274 {
275  m_pps_events.clear();
276 
277  // Generate locked pilot tone.
278  m_psin = sin(m_phase);
279  m_pcos = cos(m_phase);
280 
281  // Generate output
282  processPhase(samples_out);
283 
284  // Multiply locked tone with input.
285  Real phasor_i = m_psin * real_in - m_pcos * imag_in;
286  Real phasor_q = m_pcos * real_in + m_psin * imag_in;
287 
288  // Actual PLL
289  process_phasor(phasor_i, phasor_q);
290 }
291 
292 void PhaseLock::process_phasor(Real& phasor_i, Real& phasor_q)
293 {
294  // Run IQ phase error through low-pass filter.
295  phasor_i = m_phasor_b0 * phasor_i
298  phasor_q = m_phasor_b0 * phasor_q
301  m_phasor_i2 = m_phasor_i1;
302  m_phasor_i1 = phasor_i;
303  m_phasor_q2 = m_phasor_q1;
304  m_phasor_q1 = phasor_q;
305 
306  // Convert I/Q ratio to estimate of phase error.
307  Real phase_err;
308  if (phasor_i > std::abs(phasor_q)) {
309  // We are within +/- 45 degrees from lock.
310  // Use simple linear approximation of arctan.
311  phase_err = phasor_q / phasor_i;
312  } else if (phasor_q > 0) {
313  // We are lagging more than 45 degrees behind the input.
314  phase_err = 1;
315  } else {
316  // We are more than 45 degrees ahead of the input.
317  phase_err = -1;
318  }
319 
320  // Detect pilot level (conservative).
321  // m_pilot_level = std::min(m_pilot_level, phasor_i);
322  m_pilot_level = phasor_i;
323 
324  // Run phase error through loop filter and update frequency estimate.
325  m_freq += m_loopfilter_b0 * phase_err
327  m_loopfilter_x1 = phase_err;
328 
329  // Limit frequency to allowable range.
331 
332  // Update locked phase.
333  m_phase += m_freq;
334  if (m_phase > 2.0 * M_PI)
335  {
336  m_phase -= 2.0 * M_PI;
337  m_pilot_periods++;
338 
339  // Generate pulse-per-second.
341  {
342  m_pilot_periods = 0;
343  }
344  }
345 
346  // Update lock status.
347  if (2 * m_pilot_level > m_minsignal)
348  {
349  if (m_lock_cnt < m_lock_delay)
350  {
351  m_lock_cnt += 1; // n
352  }
353  }
354  else
355  {
356  m_lock_cnt = 0;
357  }
358 
359  // Drop PPS events when pilot not locked.
360  if (m_lock_cnt < m_lock_delay) {
361  m_pilot_periods = 0;
362  m_pps_cnt = 0;
363  m_pps_events.clear();
364  }
365 
366  // Update sample counter.
367  m_sample_cnt += 1; // n
368 }
#define M_PI
Definition: phaselock.cpp:23
Real m_phasor_q1
Definition: phaselock.h:104
Real m_phasor_b0
Definition: phaselock.h:103
Fixed< IntType, IntBits > cos(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2271
std::vector< PpsEvent > m_pps_events
Definition: phaselock.h:115
double block_position
Definition: phaselock.h:36
Real m_phasor_a1
Definition: phaselock.h:103
Real m_maxfreq
Definition: phaselock.h:102
void configure(Real freq, Real bandwidth, Real minsignal)
Definition: phaselock.cpp:87
Real m_minsignal
Definition: phaselock.h:108
int m_lock_delay
Definition: phaselock.h:110
static const int pilot_frequency
Definition: phaselock.h:29
Real m_loopfilter_b1
Definition: phaselock.h:105
Real m_psin
Definition: phaselock.h:93
Fixed< IntType, IntBits > abs(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2313
int m_pilot_periods
Definition: phaselock.h:112
Fixed< IntType, IntBits > exp(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2289
Real m_freq
Definition: phaselock.h:107
Real m_pcos
Definition: phaselock.h:94
Real m_phasor_q2
Definition: phaselock.h:104
virtual void processPhase(Real *samples_out) const
Definition: phaselock.h:99
quint64 m_pps_cnt
Definition: phaselock.h:113
void process(const std::vector< Real > &samples_in, std::vector< Real > &samples_out)
Definition: phaselock.cpp:148
quint64 sample_index
Definition: phaselock.h:35
Real m_loopfilter_b0
Definition: phaselock.h:105
Fixed< IntType, IntBits > sin(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2265
int32_t i
Definition: decimators.h:244
int m_lock_cnt
Definition: phaselock.h:111
void process_phasor(Real &phasor_i, Real &phasor_q)
Definition: phaselock.cpp:292
Real m_pilot_level
Definition: phaselock.h:109
Real m_phase
Definition: phaselock.h:92
Real m_minfreq
Definition: phaselock.h:102
Real m_phasor_i2
Definition: phaselock.h:104
Real m_phasor_a2
Definition: phaselock.h:103
Real m_phasor_i1
Definition: phaselock.h:104
Real m_loopfilter_x1
Definition: phaselock.h:106
quint64 m_sample_cnt
Definition: phaselock.h:114
float Real
Definition: dsptypes.h:42
T max(const T &x, const T &y)
Definition: framework.h:446
PhaseLock(Real freq, Real bandwidth, Real minsignal)
Definition: phaselock.cpp:26
T min(const T &x, const T &y)
Definition: framework.h:440